
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_common.c
  * @author     baiyang
  * @date       2022-11-4
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gcs.h"
#include "mav_routing.h"

#include <rtthread.h>

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_imu/sensor_imu.h>
#include <sensor_gps/sensor_gps.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#endif

#include <rtc/gp_rtc.h>
#include <uITC/uITC.h>
#include <notify/notify.h>
#include <uITC/uITC_msg.h>
#include <ahrs/ahrs_view.h>
#include <vehicle/vehicle_type.h>
#include <common/console/console.h>
#include <srv_channel/srv_channel.h>
#include <serial_manager/gp_serial.h>
#include <analogin_manager/analogin.h>
#include <internal_error/internal_error.h>
#include <battery_monitor/battery_monitor.h>
/*-----------------------------------macro------------------------------------*/
#define loop_period_us 2000

#ifndef MIN
#define MIN(a, b) (a < b ? a : b)
#endif
#ifndef MAX
#define MAX(a, b) (a > b ? a : b)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void gcsmav_send_scaled_pressure_instance(gcs_mavlink_t gcsmav, uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff));
static void gcsmav_send_battery_status2(gcs_mavlink_t gcsmav, const uint8_t instance, uitc_battery_state* battery_state);
static void gcsmav_send_mission_current(gcs_mavlink_t gcsmav, uint16_t seq);
static MISSION_STATE gcsmav_mission_state();
/*----------------------------------variable----------------------------------*/
#if MB_GCS_FTP_ENABLED
extern struct ftp_state gcs_ftp;
#endif

struct LastRadioStatus gcs_last_radio_status;
uint8_t gcs_mavlink_active;
uint8_t gcs_chan_is_streaming;
uint32_t gcs_reserve_param_space_start_ms;

// private channels are ones used for point-to-point protocols, and
// don't get broadcasts or fwded packets
uint8_t gcs_mavlink_private;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void gcsmav_ctor(gcs_mavlink_t gcsmav, gcs_mavlink_ops_t front_ops, gcs_mavlink_parameters_t parameters, rt_device_t uart)
{
    rt_memset(gcsmav, 0, sizeof(struct gcs_mavlink));

    gcsmav->ops = front_ops;

    gcsmav->mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE;
    gcsmav->_port = uart;

    gcsmav->streamRates = &parameters->streamRates[0];

    gcsmav->_timesync_request.interval_ms = 10000;
    gcsmav->last_txbuf = 100;

    gcsmav->deferred_message[0].id = MAV_MSG_HEARTBEAT;
    gcsmav->deferred_message[1].id = MAV_MSG_NEXT_PARAM;
    gcsmav->deferred_message[2].id = MAV_MSG_HIGH_LATENCY2;

    gcsmav->next_deferred_message_to_send_cache = -1;

    for (uint16_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message_bucket); i++) {
        mb_bitmask_init(&gcsmav->deferred_message_bucket[i].mb_message_ids, MAV_MSG_LAST, gcsmav->deferred_message_bucket[i].bits);
    }

    gcsmav->sending_bucket_id = gcs_no_bucket_to_send;

    mb_bitmask_init(&gcsmav->bucket_message_ids_to_send, MAV_MSG_LAST, gcsmav->bucket_message_ids_bits);
    mb_bitmask_init(&gcsmav->pushed_mb_message_ids, MAV_MSG_LAST, gcsmav->pushed_mb_message_ids_bits);

    jitter_correction(&gcsmav->lag_correction, 500, 100);
}

bool gcsmav_init(gcs_mavlink_t gcsmav, uint8_t instance)
{
    // get associated mavlink channel
    gcsmav->chan = (mavlink_channel_t)(MAVLINK_COMM_0 + instance);
    if (!mavcomm_valid_channel(gcsmav->chan)) {
        return false;
    }

    mavcomm_init_chan_locks(gcsmav->chan);

    mavlink_comm_port[gcsmav->chan] = gcsmav->_port;

    mavlink_status_t *status = mavlink_get_channel_status(gcsmav->chan);
    if (status == NULL) {
        return false;
    }

    const enum SerialProtocol mavlink_protocol = SerialManager_get_protocol(gcsmav->_port);

    if (mavlink_protocol == SerialProtocol_MAVLink2 ||
        mavlink_protocol == SerialProtocol_MAVLinkHL) {
        // load signing key
        gcsmav_load_signing_key(gcsmav);
    } else {
        // user has asked to only send MAVLink1
        status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
    }

#if HAL_HIGH_LATENCY2_ENABLED
    if (mavlink_protocol == SerialProtocol_MAVLinkHL) {
        gcsmav->is_high_latency_link = true;
    }
#endif

    return true;
}

// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels    
uint8_t gcsmav_active_channel_mask(void)
{
    return gcs_mavlink_active;
}

// return a bitmap of streaming channels
uint8_t gcsmav_streaming_channel_mask(void)
{

    return gcs_chan_is_streaming;
}

// return a bitmap of private channels
uint8_t gcsmav_private_channel_mask(void)
{
    return gcs_mavlink_private;
}

// set a channel as private. Private channels get sent heartbeats, but
// don't get broadcast packets or forwarded packets
void gcsmav_set_channel_private(mavlink_channel_t chan)
{
    const uint8_t mask = (1U<<(unsigned)chan);
    gcs_mavlink_private |= mask;
}

// return true if channel is private
bool gcsmav_is_private(mavlink_channel_t chan)
{
    return (gcs_mavlink_private & (1U<<(unsigned)chan)) != 0;
}

/// Check for available transmit space
uint16_t gcsmav_txspace(gcs_mavlink_t gcsmav)
{
    if (gcsmav->_locked) {
        return 0;
    }
    // there were concerns over return a too-large value for
    // txspace (in case we tried to do too much with the space in
    // a single loop):
    return MIN(SerialManager_tx_space(gcsmav->_port), 8192U);
}

/*
  check there is enough space for a message
 */
bool gcsmav_check_payload_size(gcs_mavlink_t gcsmav, uint16_t max_payload_len)
{
    if (gcsmav_txspace(gcsmav) < (unsigned)(gcsmav_packet_overhead(gcsmav)+max_payload_len)) {
        gcs_out_of_space_to_send(gcsmav->chan);
        return false;
    }

    return true;
}

/*
    send a statustext message to specific MAVLink connections in a bitmask

    must be called with semaphore held
 */
void gcsmav_service_statustext(gcs_mavlink_t gcsmav)
{
    status_text_queue_t _statustext_queue = gcs_statustext_queue();

    const uint8_t chan_bit = (1U<<gcsmav->chan);

    // note the lack of idx++ here.  We may remove the iteration item
    // from the queue as the last thing we do, in which case we don't
    // want to move idx.
    const uint16_t payload_size = MAV_PAYLOAD_SIZE(gcsmav->chan, STATUSTEXT);

    for (uint8_t idx=0; idx<status_text_queue_available(_statustext_queue); ) {
        rt_mutex_take(mavcomm_chan_lock(gcsmav->chan), RT_WAITING_FOREVER);

        if (gcsmav_txspace(gcsmav) < payload_size) {
            rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));
            break;
        }
        struct statustext_t *statustext = &_statustext_queue->_buffer[idx];
        if (statustext == NULL) {
            INTERNAL_ERROR(internal_error_flow_of_control);
            rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));
            break;
        }

        // check to see if we need to send this queue entry:
        if (statustext->bitmask & chan_bit) {
            mavlink_msg_statustext_send(gcsmav->chan,
                                        statustext->msg.severity,
                                        statustext->msg.text,
                                        statustext->msg.id,
                                        statustext->msg.chunk_seq);
            // indicate we've sent the message:
            statustext->bitmask &= ~chan_bit;

            if (statustext->bitmask == 0) {
                // sent everywhere it needs to be sent, remove it from the
                // queue but leave idx as-is as we want to handle the
                // remaining items which have been bumped up to out
                // current index
                status_text_queue_remove(_statustext_queue, idx);

                rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));
                continue;
            }
        }
        // this item still has places to go.  Continue to iterate over the queue
        idx++;

        rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));
    }
}

// queue a message to be sent (try_send_message does the *actual*
// mavlink work!)
void gcsmav_send_message(gcs_mavlink_t gcsmav, enum mb_message id)
{
    if (id == MAV_MSG_HEARTBEAT || id == MAV_MSG_HIGH_LATENCY2) {
        gcsmav_save_signing_timestamp(false);
        // update the mask of all streaming channels
        if (gcsmav_is_streaming(gcsmav)) {
            gcs_chan_is_streaming |= (1U<<(gcsmav->chan-MAVLINK_COMM_0));
        } else {
            gcs_chan_is_streaming &= ~(1U<<(gcsmav->chan-MAVLINK_COMM_0));
        }
    }

    mb_bitmask_set(&gcsmav->pushed_mb_message_ids, id);
}

void gcsmav_send_text(gcs_mavlink_t gcsmav, MAV_SEVERITY severity, const char *fmt, ...)
{
    va_list arg_list;
    va_start(arg_list, fmt);
    gcs_send_textv2(severity, fmt, arg_list, (1<<gcsmav->chan));
    va_end(arg_list);
}

void gcsmav_update_send(gcs_mavlink_t gcsmav)
{
    //send_ftp_replies();

    if (!gcsmav->deferred_messages_initialised) {
        gcsmav_initialise_message_intervals_from_streamrates(gcsmav);
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
        //gcsmav_initialise_message_intervals_from_config_files();
#endif
        gcsmav->deferred_messages_initialised = true;
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    uint32_t retry_deferred_body_start = time_micros();
#endif

    const uint32_t start = time_millis();
    const uint16_t start16 = start & 0xFFFF;
    while (time_millis() - start < 5) { // spend a max of 5ms sending messages.  This should never trigger - out_of_time() should become true
        if (gcs_out_of_time()) {
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
            gcsmav->try_send_message_stats.out_of_time++;
#endif
            break;
        }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
        retry_deferred_body_start = time_micros();
#endif

        // check if any "specially handled" messages should be sent out
        {
            const int8_t next = gcsmav_deferred_message_to_send_index(gcsmav, start16);
            if (next != -1) {
                if (!gcsmav_do_try_send_message(gcsmav, gcsmav->deferred_message[next].id)) {
                    break;
                }
                // we try to keep output on a regular clock to avoid
                // user support questions:
                const uint16_t interval_ms = gcsmav->deferred_message[next].interval_ms;
                gcsmav->deferred_message[next].last_sent_ms += interval_ms;
                // but we do not want to try to catch up too much:
                if ((uint16_t)(start16 - gcsmav->deferred_message[next].last_sent_ms) > interval_ms) {
                    gcsmav->deferred_message[next].last_sent_ms = start16;
                }

                gcsmav->next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
                const uint32_t stop = time_micros();
                const uint32_t delta = stop - retry_deferred_body_start;
                if (delta > gcsmav->try_send_message_stats.max_retry_deferred_body_us) {
                    gcsmav->try_send_message_stats.max_retry_deferred_body_us = delta;
                    gcsmav->try_send_message_stats.max_retry_deferred_body_type = 1;
                }
#endif
                continue;
            }
        }

        // check for any messages that the code has explicitly sent
        const int16_t fs = mb_bitmask_first_set(&gcsmav->pushed_mb_message_ids);
        if (fs != -1) {
            enum mb_message next = (enum mb_message)fs;
            if (!gcsmav_do_try_send_message(gcsmav, next)) {
                break;
            }
            mb_bitmask_clear(&gcsmav->pushed_mb_message_ids, next);
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
            const uint32_t stop = time_micros();
            const uint32_t delta = stop - retry_deferred_body_start;
            if (delta > gcsmav->try_send_message_stats.max_retry_deferred_body_us) {
                gcsmav->try_send_message_stats.max_retry_deferred_body_us = delta;
                gcsmav->try_send_message_stats.max_retry_deferred_body_type = 2;
            }
#endif
            continue;
        }

        enum mb_message next = gcsmav_next_deferred_bucket_message_to_send(gcsmav, start16);
        if (next != gcs_no_message_to_send) {
            if (!gcsmav_do_try_send_message(gcsmav, next)) {
                break;
            }
            mb_bitmask_clear(&gcsmav->bucket_message_ids_to_send, next);
            if (mb_bitmask_count(&gcsmav->bucket_message_ids_to_send) == 0) {
                // we sent everything in the bucket.  Reschedule it.
                // we try to keep output on a regular clock to avoid
                // user support questions:
                const uint16_t interval_ms = gcsmav_get_reschedule_interval_ms(gcsmav, &gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id]);
                gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id].last_sent_ms += interval_ms;
                // but we do not want to try to catch up too much:
                if ((uint16_t)(start16 - gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id].last_sent_ms) > interval_ms) {
                    gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id].last_sent_ms = start16;
                }
                gcsmav_find_next_bucket_to_send(gcsmav, start16);
            }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
                const uint32_t stop = time_micros();
                const uint32_t delta = stop - retry_deferred_body_start;
                if (delta > gcsmav->try_send_message_stats.max_retry_deferred_body_us) {
                    gcsmav->try_send_message_stats.max_retry_deferred_body_us = delta;
                    gcsmav->try_send_message_stats.max_retry_deferred_body_type = 3;
                }
#endif
            continue;
        }
        break;
    }
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    const uint32_t stop = time_micros();
    const uint32_t delta = stop - retry_deferred_body_start;
    if (delta > gcsmav->try_send_message_stats.max_retry_deferred_body_us) {
        gcsmav->try_send_message_stats.max_retry_deferred_body_us = delta;
        gcsmav->try_send_message_stats.max_retry_deferred_body_type = 4;
    }
#endif

    // update the number of packets transmitted base on seqno, making
    // the assumption that we don't send more than 256 messages
    // between the last pass through here
    mavlink_status_t *status = mavlink_get_channel_status(gcsmav->chan);
    if (status != NULL) {
        gcsmav->send_packet_count += (uint8_t)(status->current_tx_seq - gcsmav->last_tx_seq);
        gcsmav->last_tx_seq = status->current_tx_seq;
    }
}

// uint32_t max_time_us=1000
void gcsmav_update_receive(gcs_mavlink_t gcsmav, uint32_t max_time_us)
{
    // do absolutely nothing if we are locked
    if (gcsmav_locked(gcsmav)) {
        return;
    }

    // receive new packets
    mavlink_message_t msg;
    mavlink_status_t status;
    uint32_t tstart_us = time_micros();
    uint32_t now_ms = time_millis();

    status.packet_rx_drop_count = 0;

    const uint16_t nbytes = SerialManager_rx_available(gcsmav->_port);
    for (uint16_t i=0; i<nbytes; i++)
    {
        const uint8_t c = (uint8_t)SerialManager_read_byte(gcsmav->_port);
        const uint32_t protocol_timeout = 4000;
        
        if (gcsmav->alternative.handler &&
            now_ms - gcsmav->alternative.last_mavlink_ms > protocol_timeout) {
            /*
              we have an alternative protocol handler installed and we
              haven't parsed a MAVLink packet for 4 seconds. Try
              parsing using alternative handler
             */
            if (gcsmav->alternative.handler(c, mavlink_comm_port[gcsmav->chan])) {
                gcsmav->alternative.last_alternate_ms = now_ms;
                gcs_alternative_active[gcsmav->chan] = true;
            }
            
            /*
              we may also try parsing as MAVLink if we haven't had a
              successful parse on the alternative protocol for 4s
             */
            if (now_ms - gcsmav->alternative.last_alternate_ms <= protocol_timeout) {
                continue;
            }
        }

        bool parsed_packet = false;

        // Try to get a new message
        if (mavlink_parse_char(gcsmav->chan, c, &msg, &status)) {
            //hal.util->persistent_data.last_mavlink_msgid = msg.msgid;
            gcsmav_packet_received(gcsmav, &status, &msg);
            parsed_packet = true;
            gcs_alternative_active[gcsmav->chan] = false;
            gcsmav->alternative.last_mavlink_ms = now_ms;
            //hal.util->persistent_data.last_mavlink_msgid = 0;
        }

        if (parsed_packet || i % 100 == 0) {
            // make sure we don't spend too much time parsing mavlink messages
            if (time_micros() - tstart_us > max_time_us) {
                break;
            }
        }
    }

    const uint32_t tnow = time_millis();

    // send a timesync message every 10 seconds; this is for data
    // collection purposes
    if (tnow - gcsmav->_timesync_request.last_sent_ms > gcsmav->_timesync_request.interval_ms && !gcsmav_is_private(gcsmav->chan)) {
        if (MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, TIMESYNC)) {
            gcsmav_send_timesync(gcsmav);
            gcsmav->_timesync_request.last_sent_ms = tnow;
        }
    }

    // consider logging mavlink stats:
    if (gcsmav_is_active(gcsmav) || gcsmav_is_streaming(gcsmav)) {
        if (tnow - gcsmav->last_mavlink_stats_logged > 1000) {
            //log_mavlink_stats();
            gcsmav->last_mavlink_stats_logged = tnow;
        }
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS

    const uint16_t now16_ms = time_millis16();

    if ((uint16_t)(now16_ms - gcsmav->try_send_message_stats.statustext_last_sent_ms) > 10000U) {
        if (gcsmav->try_send_message_stats.longest_time_us) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): mb_msg=%u took %uus to send",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.longest_id,
                            gcsmav->try_send_message_stats.longest_time_us);
            gcsmav->try_send_message_stats.longest_time_us = 0;
        }
        if (gcsmav->try_send_message_stats.no_space_for_message &&
            (gcsmav_is_active(gcsmav) || gcsmav_is_streaming(gcsmav))) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): out-of-space: %u",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.no_space_for_message);
            gcsmav->try_send_message_stats.no_space_for_message = 0;
        }
        if (gcsmav->try_send_message_stats.out_of_time) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): out-of-time=%u",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.out_of_time);
            gcsmav->try_send_message_stats.out_of_time = 0;
        }
        if (gcsmav->max_slowdown_ms) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): max slowdown=%u",
                            gcsmav->chan,
                            gcsmav->max_slowdown_ms);
            gcsmav->max_slowdown_ms = 0;
        }
        if (gcsmav->try_send_message_stats.behind) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): behind=%u",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.behind);
            gcsmav->try_send_message_stats.behind = 0;
        }
        if (gcsmav->try_send_message_stats.fnbts_maxtime) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): fnbts_maxtime=%uus",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.fnbts_maxtime);
            gcsmav->try_send_message_stats.fnbts_maxtime = 0;
        }
        if (gcsmav->try_send_message_stats.max_retry_deferred_body_us) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "GCS.chan(%u): retry_body_maxtime=%uus (%u)",
                            gcsmav->chan,
                            gcsmav->try_send_message_stats.max_retry_deferred_body_us,
                            gcsmav->try_send_message_stats.max_retry_deferred_body_type
                );
            gcsmav->try_send_message_stats.max_retry_deferred_body_us = 0;
        }

        if (ARRAY_SIZE(gcsmav->deferred_message_bucket) >= 5) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "B1. intvl. (%u): %u %u %u %u %u",
                            gcsmav->chan,
                            gcsmav->deferred_message_bucket[0].interval_ms,
                            gcsmav->deferred_message_bucket[1].interval_ms,
                            gcsmav->deferred_message_bucket[2].interval_ms,
                            gcsmav->deferred_message_bucket[3].interval_ms,
                            gcsmav->deferred_message_bucket[4].interval_ms);
        }

        if (ARRAY_SIZE(gcsmav->deferred_message_bucket) >= 10) {
            gcs_send_text(MAV_SEVERITY_INFO,
                            "B2. intvl. (%u): %u %u %u %u %u",
                            gcsmav->chan,
                            gcsmav->deferred_message_bucket[5].interval_ms,
                            gcsmav->deferred_message_bucket[6].interval_ms,
                            gcsmav->deferred_message_bucket[7].interval_ms,
                            gcsmav->deferred_message_bucket[8].interval_ms,
                            gcsmav->deferred_message_bucket[9].interval_ms);
        }

        gcsmav->try_send_message_stats.statustext_last_sent_ms = now16_ms;
    }
#endif
}

void gcsmav_packetReceived(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg)
{
    // we exclude radio packets because we historically used this to
    // make it possible to use the CLI over the radio
    if (msg->msgid != MAVLINK_MSG_ID_RADIO && msg->msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
        gcs_mavlink_active |= (1U<<(gcsmav->chan-MAVLINK_COMM_0));
    }

    enum SerialProtocol mavlink_protocol = SerialManager_get_protocol(gcsmav->_port);
    if (!(status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
        (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
        (mavlink_protocol == SerialProtocol_MAVLink2 ||
         mavlink_protocol == SerialProtocol_MAVLinkHL)) {
        // if we receive any MAVLink2 packets on a connection
        // currently sending MAVLink1 then switch to sending
        // MAVLink2
        mavlink_status_t *cstatus = mavlink_get_channel_status(gcsmav->chan);
        if (cstatus != NULL) {
            cstatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
        }
    }
    if (!mavrouting_check_and_forward(gcsmav->chan, msg)) {
        // the routing code has indicated we should not handle this packet locally
        return;
    }
    if (msg->msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
#if HAL_MOUNT_ENABLED
        // allow mounts to see the location of other vehicles
        handle_mount_message(msg);
#endif
    }
    if (!gcsmav_accept_packet(gcsmav, status, msg)) {
        // e.g. enforce-sysid says we shouldn't look at this packet
        return;
    }

    gcsmav_handle_message(gcsmav, msg);
}

void gcsmav_initialise_message_intervals_from_streamrates(gcs_mavlink_t gcsmav)
{
    // this is O(n^2), but it's once at boot and across a 10-entry list...
    for (uint8_t i=0; all_stream_entries[i].mb_message_ids != NULL; i++) {
        gcsmav_initialise_message_intervals_for_stream(gcsmav, all_stream_entries[i].stream_id);
    }
#if HAL_HIGH_LATENCY2_ENABLED
    if (!gcsmav->is_high_latency_link) {
        gcsmav_set_mavlink_message_id_interval(gcsmav, MAVLINK_MSG_ID_HEARTBEAT, 1000);
    } else {
        gcsmav_set_mavlink_message_id_interval(gcsmav, MAVLINK_MSG_ID_HIGH_LATENCY2, 5000);
    }
#else
    gcsmav_set_mavlink_message_id_interval(gcsmav, MAVLINK_MSG_ID_HEARTBEAT, 1000);
#endif
}

bool gcsmav_get_default_interval_for_mb_message(gcs_mavlink_t gcsmav, const enum mb_message id, uint16_t *interval)
{
    if (id == MAV_MSG_HEARTBEAT) {
        // handle heartbeat requests as a special case because heartbeat is not "streamed"
        *interval = 1000;
        return true;
    }

    if (id == MAV_MSG_HIGH_LATENCY2) {
        // handle HL2 requests as a special case because HL2 is not "streamed"
        *interval = 5000;
        return true;
    }

#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
    // a user can specify default rates in files, which are read close
    // to vehicle startup
    if (default_intervals_from_files != NULL &&
        default_intervals_from_files->get_interval_for_ap_message_id(id, interval)) {
        return true;
    }
#endif

    // find which stream this ap_message is in
    for (uint8_t i=0; all_stream_entries[i].mb_message_ids != NULL; i++) {
        const struct stream_entries *entries = &all_stream_entries[i];
        for (uint8_t j=0; j<entries->num_mb_message_ids; j++) {
            if (entries->mb_message_ids[j] == id) {
                *interval = gcsmav_get_interval_for_stream(gcsmav, all_stream_entries[i].stream_id);
                return true;
            }
        }
    }
    return false;
}

void gcsmav_initialise_message_intervals_for_stream(gcs_mavlink_t gcsmav, enum mav_streams id)
{
    for (uint8_t i=0; all_stream_entries[i].mb_message_ids != NULL; i++) {
        const struct stream_entries *entries = &all_stream_entries[i];
        if (entries->stream_id != id) {
            continue;
        }
        // found it!
        const uint16_t interval_ms = gcsmav_get_interval_for_stream(gcsmav, id);
        for (uint8_t j=0; j<entries->num_mb_message_ids; j++) {
            gcsmav_set_mb_message_interval(gcsmav, entries->mb_message_ids[j], interval_ms);
        }
        break;
    }
}

uint16_t gcsmav_get_interval_for_stream(gcs_mavlink_t gcsmav, enum mav_streams id)
{
    const int16_t frate = gcsmav->streamRates[id];
    if (frate == 0) {
        return 0;
    }
    const uint32_t ret = 1000/frate;
    if (ret > 60000) {
        return 60000;
    }
    return ret;
}

bool gcsmav_set_mb_message_interval(gcs_mavlink_t gcsmav, enum mb_message id, uint16_t interval_ms)
{
    if (id == MAV_MSG_NEXT_PARAM) {
        // force parameters to *always* get streamed so a vehicle is
        // recoverable from bad configuration:
        if (interval_ms == 0) {
            interval_ms = 100;
        } else if (interval_ms > 1000) {
            interval_ms = 1000;
        }
    }

    // send messages out at most 80% of main loop rate
    if (interval_ms != 0 &&
        interval_ms*800 < loop_period_us) {
        interval_ms = loop_period_us/800.0f;
    }

    // check if it's a specially-handled message:
    const int8_t deferred_offset = gcsmav_get_deferred_message_index(gcsmav, id);
    if (deferred_offset != -1) {
        gcsmav->deferred_message[deferred_offset].interval_ms = interval_ms;
        gcsmav->deferred_message[deferred_offset].last_sent_ms = time_millis16();
        return true;
    }

    // see which bucket has the closest interval:
    int8_t closest_bucket = -1;
    uint16_t closest_bucket_interval_delta = UINT16_MAX;
    int8_t in_bucket = -1;
    int8_t empty_bucket_id = -1;
    for (uint8_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message_bucket); i++) {
        const struct deferred_message_bucket_t *bucket = &gcsmav->deferred_message_bucket[i];
        if (bucket->interval_ms == 0) {
            // unused bucket
            if (empty_bucket_id == -1) {
                empty_bucket_id = i;
            }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
            if (mb_bitmask_count(&bucket->mb_message_ids) != 0) {
                console_panic("Bucket %u has zero interval but with ids set", i);
            }
#endif
            continue;
        }
        if (mb_bitmask_get(&bucket->mb_message_ids, id)) {
            in_bucket = i;
        }
        const uint16_t interval_delta = abs(bucket->interval_ms - interval_ms);
        if (interval_delta < closest_bucket_interval_delta) {
            closest_bucket = i;
            closest_bucket_interval_delta = interval_delta;
        }
    }

    if (in_bucket == -1 && interval_ms == 0) {
        // not in a bucket and told to remove from scheduling
        return true;
    }

    if (in_bucket != -1) {
        if (interval_ms == 0) {
            // remove it
            gcsmav_remove_message_from_bucket(gcsmav, in_bucket, id);
            return true;
        }
        if (closest_bucket_interval_delta == 0 &&
            in_bucket == closest_bucket) {
            // don't need to move it
            return true;
        }
        // remove from existing bucket
        gcsmav_remove_message_from_bucket(gcsmav, in_bucket, id);
        if (empty_bucket_id == -1 &&
            mb_bitmask_count(&gcsmav->deferred_message_bucket[in_bucket].mb_message_ids) == 0) {
            empty_bucket_id = in_bucket;
        }
    }

    if (closest_bucket == -1 && empty_bucket_id == -1) {
        // gah?!
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        console_printf("no buckets?!\n");
#endif
        return false;
    }

    if (closest_bucket_interval_delta != 0 &&
        empty_bucket_id != -1) {
        // allocate a bucket for this interval
        gcsmav->deferred_message_bucket[empty_bucket_id].interval_ms = interval_ms;
        gcsmav->deferred_message_bucket[empty_bucket_id].last_sent_ms = time_millis16();
        closest_bucket = empty_bucket_id;
    }

    mb_bitmask_set(&gcsmav->deferred_message_bucket[closest_bucket].mb_message_ids, id);

    if (gcsmav->sending_bucket_id == gcs_no_bucket_to_send) {
        gcsmav->sending_bucket_id = closest_bucket;
        mb_bitmask_copy(&gcsmav->bucket_message_ids_to_send, &gcsmav->deferred_message_bucket[closest_bucket].mb_message_ids);
    }

    return true;
}

int8_t gcsmav_get_deferred_message_index(gcs_mavlink_t gcsmav, const enum mb_message id)
{
    for (uint8_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message); i++) {
        if (gcsmav->deferred_message[i].id == id) {
            return i;
        }
    }
    return -1;
}

void gcsmav_remove_message_from_bucket(gcs_mavlink_t gcsmav, int8_t bucket, enum mb_message id)
{
    mb_bitmask_clear(&gcsmav->deferred_message_bucket[bucket].mb_message_ids , id);
    if (mb_bitmask_count(&gcsmav->deferred_message_bucket[bucket].mb_message_ids) == 0) {
        // bucket empty.  Free it:
        gcsmav->deferred_message_bucket[bucket].interval_ms = 0;
        gcsmav->deferred_message_bucket[bucket].last_sent_ms = 0;
    }

    if (bucket == gcsmav->sending_bucket_id) {
        mb_bitmask_clear(&gcsmav->bucket_message_ids_to_send, id);
        if (mb_bitmask_count(&gcsmav->bucket_message_ids_to_send) == 0) {
            gcsmav_find_next_bucket_to_send(gcsmav, time_millis16());
        } else {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
            if (gcsmav->deferred_message_bucket[bucket].interval_ms == 0 &&
                gcsmav->deferred_message_bucket[bucket].last_sent_ms == 0) {
                // we just freed this bucket!  this would mean that
                // somehow our messages-still-to-send was a superset
                // of the messages in the bucket we were sending,
                // which would be bad.
                INTERNAL_ERROR(internal_error_flow_of_control);
            }
#endif
        }
    }
}

// typical runtime on fmuv3: 5 microseconds for 3 buckets
void gcsmav_find_next_bucket_to_send(gcs_mavlink_t gcsmav, uint16_t now16_ms)
{
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    /* disable interrupt */
    rt_base_t level = rt_hw_interrupt_disable();
    uint32_t start_us = time_micros();
#endif

    // all done sending this bucket... find another bucket...
    gcsmav->sending_bucket_id = gcs_no_bucket_to_send;
    uint16_t ms_before_send_next_bucket_to_send = UINT16_MAX;
    for (uint8_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message_bucket); i++) {
        if (mb_bitmask_count(&gcsmav->deferred_message_bucket[i].mb_message_ids) == 0) {
            // no entries
            continue;
        }
        const uint16_t interval = gcsmav_get_reschedule_interval_ms(gcsmav, &gcsmav->deferred_message_bucket[i]);
        const uint16_t ms_since_last_sent = now16_ms - gcsmav->deferred_message_bucket[i].last_sent_ms;
        uint16_t ms_before_send_this_bucket;
        if (ms_since_last_sent > interval) {
            // should already have sent this bucket!
            ms_before_send_this_bucket = 0;
        } else {
            ms_before_send_this_bucket = interval - ms_since_last_sent;
        }
        if (ms_before_send_this_bucket < ms_before_send_next_bucket_to_send) {
            gcsmav->sending_bucket_id = i;
            ms_before_send_next_bucket_to_send = ms_before_send_this_bucket;
        }
    }
    if (gcsmav->sending_bucket_id != gcs_no_bucket_to_send) {
        mb_bitmask_copy(&gcsmav->bucket_message_ids_to_send, &gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id].mb_message_ids);
    } else {
        mb_bitmask_clearall(&gcsmav->bucket_message_ids_to_send);
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    uint32_t delta_us = time_micros() - start_us;
    /* enable interrupt */
    rt_hw_interrupt_enable(level);
    if (delta_us > gcsmav->try_send_message_stats.fnbts_maxtime) {
        gcsmav->try_send_message_stats.fnbts_maxtime = delta_us;
    }
#endif
}

uint16_t gcsmav_get_reschedule_interval_ms(gcs_mavlink_t gcsmav, const struct deferred_message_bucket_t *deferred)
{
    uint32_t interval_ms = deferred->interval_ms;

    interval_ms += gcsmav->stream_slowdown_ms;

    // slow most messages down if we're transfering parameters or
    // waypoints:
    if (gcsmav->_queued_parameter) {
        // we are sending parameters, penalize streams:
        interval_ms *= 4;
    }

#if 0
    if (requesting_mission_items()) {
        // we are sending requests for waypoints, penalize streams:
        interval_ms *= 4;
    }
#endif

#if MB_GCS_FTP_ENABLED
    if (time_millis() - gcs_ftp.last_send_ms < 500) {
        // we are sending ftp replies
        interval_ms *= 4;
    }
#endif

    if (interval_ms > 60000) {
        return 60000;
    }

    return interval_ms;
}

bool gcsmav_set_mavlink_message_id_interval(gcs_mavlink_t gcsmav, const uint32_t mavlink_id, const uint16_t interval_ms)
{
    const enum mb_message id = gcsmav_mavlink_id_to_ap_message_id(mavlink_id);
    if (id == MAV_MSG_LAST) {
        gcs_send_text(MAV_SEVERITY_INFO, "No mb_message for mavlink id (%u)", (unsigned int)mavlink_id);
        return false;
    }
    return  gcsmav_set_mb_message_interval(gcsmav, id, interval_ms);
}

enum mb_message gcsmav_mavlink_id_to_ap_message_id(const uint32_t mavlink_id)
{
    // MSG_NEXT_MISSION_REQUEST doesn't correspond to a mavlink message directly.
    // It is used to request the next waypoint after receiving one.

    // MSG_NEXT_PARAM doesn't correspond to a mavlink message directly.
    // It is used to send the next parameter in a stream after sending one

    // MSG_NAMED_FLOAT messages can't really be "streamed"...

    static const struct {
        uint32_t mavlink_id;
        enum mb_message msg_id;
    } map[] = {
        { MAVLINK_MSG_ID_HEARTBEAT,             MAV_MSG_HEARTBEAT},
        { MAVLINK_MSG_ID_ATTITUDE,              MAV_MSG_ATTITUDE},
        { MAVLINK_MSG_ID_ATTITUDE_QUATERNION,   MAV_MSG_ATTITUDE_QUATERNION},
        { MAVLINK_MSG_ID_GLOBAL_POSITION_INT,   MAV_MSG_LOCATION},
        { MAVLINK_MSG_ID_HOME_POSITION,         MAV_MSG_HOME},
        { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN,     MAV_MSG_ORIGIN},
        { MAVLINK_MSG_ID_SYS_STATUS,            MAV_MSG_SYS_STATUS},
        { MAVLINK_MSG_ID_POWER_STATUS,          MAV_MSG_POWER_STATUS},
#if HAL_WITH_MCU_MONITORING
        { MAVLINK_MSG_ID_MCU_STATUS,            MAV_MSG_MCU_STATUS},
#endif
        { MAVLINK_MSG_ID_MEMINFO,               MAV_MSG_MEMINFO},
        { MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, MAV_MSG_NAV_CONTROLLER_OUTPUT},
        { MAVLINK_MSG_ID_MISSION_CURRENT,       MAV_MSG_CURRENT_WAYPOINT},
        { MAVLINK_MSG_ID_VFR_HUD,               MAV_MSG_VFR_HUD},
        { MAVLINK_MSG_ID_SERVO_OUTPUT_RAW,      MAV_MSG_SERVO_OUTPUT_RAW},
        { MAVLINK_MSG_ID_RC_CHANNELS,           MAV_MSG_RC_CHANNELS},
        { MAVLINK_MSG_ID_RC_CHANNELS_RAW,       MAV_MSG_RC_CHANNELS_RAW},
        { MAVLINK_MSG_ID_RAW_IMU,               MAV_MSG_RAW_IMU},
        { MAVLINK_MSG_ID_SCALED_IMU,            MAV_MSG_SCALED_IMU},
        { MAVLINK_MSG_ID_SCALED_IMU2,           MAV_MSG_SCALED_IMU2},
        { MAVLINK_MSG_ID_SCALED_IMU3,           MAV_MSG_SCALED_IMU3},
        { MAVLINK_MSG_ID_SCALED_PRESSURE,       MAV_MSG_SCALED_PRESSURE},
        { MAVLINK_MSG_ID_SCALED_PRESSURE2,      MAV_MSG_SCALED_PRESSURE2},
        { MAVLINK_MSG_ID_SCALED_PRESSURE3,      MAV_MSG_SCALED_PRESSURE3},
        { MAVLINK_MSG_ID_GPS_RAW_INT,           MAV_MSG_GPS_RAW},
        { MAVLINK_MSG_ID_GPS_RTK,               MAV_MSG_GPS_RTK},
        { MAVLINK_MSG_ID_GPS2_RAW,              MAV_MSG_GPS2_RAW},
        { MAVLINK_MSG_ID_GPS2_RTK,              MAV_MSG_GPS2_RTK},
        { MAVLINK_MSG_ID_SYSTEM_TIME,           MAV_MSG_SYSTEM_TIME},
        { MAVLINK_MSG_ID_RC_CHANNELS_SCALED,    MAV_MSG_SERVO_OUT},
        { MAVLINK_MSG_ID_PARAM_VALUE,           MAV_MSG_NEXT_PARAM},
        { MAVLINK_MSG_ID_FENCE_STATUS,          MAV_MSG_FENCE_STATUS},
        { MAVLINK_MSG_ID_AHRS,                  MAV_MSG_AHRS},
#if AP_SIM_ENABLED
        { MAVLINK_MSG_ID_SIMSTATE,              MAV_MSG_SIMSTATE},
        { MAVLINK_MSG_ID_SIM_STATE,             MAV_MSG_SIM_STATE},
#endif
        { MAVLINK_MSG_ID_AHRS2,                 MAV_MSG_AHRS2},
        { MAVLINK_MSG_ID_HWSTATUS,              MAV_MSG_HWSTATUS},
        { MAVLINK_MSG_ID_WIND,                  MAV_MSG_WIND},
        { MAVLINK_MSG_ID_RANGEFINDER,           MAV_MSG_RANGEFINDER},
        { MAVLINK_MSG_ID_DISTANCE_SENSOR,       MAV_MSG_DISTANCE_SENSOR},
            // request also does report:
        { MAVLINK_MSG_ID_TERRAIN_REQUEST,       MAV_MSG_TERRAIN},
#if MB_MAVLINK_BATTERY2_ENABLED
        { MAVLINK_MSG_ID_BATTERY2,              MAV_MSG_BATTERY2},
#endif
        { MAVLINK_MSG_ID_CAMERA_FEEDBACK,       MAV_MSG_CAMERA_FEEDBACK},
#if HAL_MOUNT_ENABLED
        { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MAV_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
        { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MAV_MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
#endif
#if MB_OPTICALFLOW_ENABLED
        { MAVLINK_MSG_ID_OPTICAL_FLOW,          MAV_MSG_OPTICAL_FLOW},
#endif
        { MAVLINK_MSG_ID_MAG_CAL_PROGRESS,      MAV_MSG_MAG_CAL_PROGRESS},
        { MAVLINK_MSG_ID_MAG_CAL_REPORT,        MAV_MSG_MAG_CAL_REPORT},
        { MAVLINK_MSG_ID_EKF_STATUS_REPORT,     MAV_MSG_EKF_STATUS_REPORT},
        { MAVLINK_MSG_ID_LOCAL_POSITION_NED,    MAV_MSG_LOCAL_POSITION},
        { MAVLINK_MSG_ID_PID_TUNING,            MAV_MSG_PID_TUNING},
        { MAVLINK_MSG_ID_VIBRATION,             MAV_MSG_VIBRATION},
#if MB_RPM_ENABLED
        { MAVLINK_MSG_ID_RPM,                   MAV_MSG_RPM},
#endif
        { MAVLINK_MSG_ID_MISSION_ITEM_REACHED,  MAV_MSG_MISSION_ITEM_REACHED},
        { MAVLINK_MSG_ID_ATTITUDE_TARGET,       MAV_MSG_ATTITUDE_TARGET},
        { MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT,  MAV_MSG_POSITION_TARGET_GLOBAL_INT},
        { MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED,  MAV_MSG_POSITION_TARGET_LOCAL_NED},
        { MAVLINK_MSG_ID_ADSB_VEHICLE,          MAV_MSG_ADSB_VEHICLE},
        { MAVLINK_MSG_ID_BATTERY_STATUS,        MAV_MSG_BATTERY_STATUS},
        { MAVLINK_MSG_ID_AOA_SSA,               MAV_MSG_AOA_SSA},
        { MAVLINK_MSG_ID_DEEPSTALL,             MAV_MSG_LANDING},
        { MAVLINK_MSG_ID_EXTENDED_SYS_STATE,    MAV_MSG_EXTENDED_SYS_STATE},
        { MAVLINK_MSG_ID_AUTOPILOT_VERSION,     MAV_MSG_AUTOPILOT_VERSION},
#if HAL_EFI_ENABLED
        { MAVLINK_MSG_ID_EFI_STATUS,            MAV_MSG_EFI_STATUS},
#endif
#if HAL_GENERATOR_ENABLED
        { MAVLINK_MSG_ID_GENERATOR_STATUS,      MAV_MSG_GENERATOR_STATUS},
#endif
        { MAVLINK_MSG_ID_WINCH_STATUS,          MAV_MSG_WINCH_STATUS},
#if HAL_WITH_ESC_TELEM
        { MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4,  MAV_MSG_ESC_TELEMETRY},
#endif
#if MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
        { MAVLINK_MSG_ID_WATER_DEPTH,           MAV_MSG_WATER_DEPTH},
#endif
#if HAL_HIGH_LATENCY2_ENABLED
        { MAVLINK_MSG_ID_HIGH_LATENCY2,         MAV_MSG_HIGH_LATENCY2},
#endif
#if AP_AIS_ENABLED
        { MAVLINK_MSG_ID_AIS_VESSEL,            MAV_MSG_AIS_VESSEL},
#endif
#if HAL_ADSB_ENABLED
        { MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_STATUS, MAV_MSG_UAVIONIX_ADSB_OUT_STATUS},
#endif
            };

    for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
        if (map[i].mavlink_id == mavlink_id) {
            return map[i].msg_id;
        }
    }
    return MAV_MSG_LAST;
}

int8_t gcsmav_deferred_message_to_send_index(gcs_mavlink_t gcsmav, uint16_t now16_ms)
{

    if (gcsmav->next_deferred_message_to_send_cache == -1) {
        uint16_t ms_before_next_message_to_send = UINT16_MAX;
        for (uint8_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message); i++) {
            const uint16_t interval_ms = gcsmav->deferred_message[i].interval_ms;
            if (interval_ms == 0) {
                continue;
            }
            const uint16_t ms_since_last_sent = now16_ms - gcsmav->deferred_message[i].last_sent_ms;
            uint16_t ms_before_send_this_message;
            if (ms_since_last_sent > interval_ms) {
                // should already have sent this one!
                ms_before_send_this_message = 0;
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
                gcsmav->try_send_message_stats.behind++;
#endif
            } else {
                ms_before_send_this_message = interval_ms - ms_since_last_sent;
            }
            if (ms_before_send_this_message < ms_before_next_message_to_send) {
                gcsmav->next_deferred_message_to_send_cache = i;
                ms_before_next_message_to_send = ms_before_send_this_message;
            }
        }
    }

    if (gcsmav->next_deferred_message_to_send_cache == -1) {
        // this really shouldn't happen; we force parameter rates, for example.
        return -1;
    }

    const uint16_t ms_since_last_sent = now16_ms - gcsmav->deferred_message[gcsmav->next_deferred_message_to_send_cache].last_sent_ms;
    if (ms_since_last_sent < gcsmav->deferred_message[gcsmav->next_deferred_message_to_send_cache].interval_ms) {
        return -1;
    }

    return gcsmav->next_deferred_message_to_send_cache;
}

bool gcsmav_do_try_send_message(gcs_mavlink_t gcsmav, const enum mb_message id)
{
    if (gcsmav_telemetry_delayed(gcsmav)) {
        return false;
    }

    rt_mutex_take(mavcomm_chan_lock(gcsmav->chan), RT_WAITING_FOREVER);
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    /* disable interrupt */
    rt_base_t level = rt_hw_interrupt_disable();
    uint32_t start_send_message_us = time_micros();
#endif

    if (!gcsmav_try_send_message(gcsmav, id)) {
        // didn't fit in buffer...
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
        gcsmav->try_send_message_stats.no_space_for_message++;

        /* enable interrupt */
        rt_hw_interrupt_enable(level);
#endif
        rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));
        return false;
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    const uint32_t delta_us = time_micros() - start_send_message_us;

    /* enable interrupt */
    rt_hw_interrupt_enable(level);
    if (delta_us > gcsmav->try_send_message_stats.longest_time_us) {
        gcsmav->try_send_message_stats.longest_time_us = delta_us;
        gcsmav->try_send_message_stats.longest_id = id;
    }
#endif

    rt_mutex_release(mavcomm_chan_lock(gcsmav->chan));

    return true;
}

// are we still delaying telemetry to try to avoid Xbee bricking?
bool gcsmav_telemetry_delayed(gcs_mavlink_t gcsmav)
{
    uint32_t tnow = time_millis() >> 10;
    if (tnow > gcsmav_telem_delay(gcsmav)) {
        return false;
    }
    if (gcsmav->chan == MAVLINK_COMM_0 && SerialManager_usb_connected()) {
        // this is USB telemetry, so won't be an Xbee
        return false;
    }
    // we're either on the 2nd UART, or no USB cable is connected
    // we need to delay telemetry by the TELEM_DELAY time
    return true;
}


bool gcsmav_TrySendMessage(gcs_mavlink_t gcsmav, const enum mb_message id)
{
    bool ret = true;

    switch(id) {

    case MAV_MSG_ATTITUDE:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, ATTITUDE);
        gcsmav_send_attitude(gcsmav);
        break;

    case MAV_MSG_ATTITUDE_QUATERNION:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, ATTITUDE_QUATERNION);
        gcsmav_send_attitude_quaternion(gcsmav);
        break;

    case MAV_MSG_NEXT_PARAM:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, PARAM_VALUE);
        gcsmav_queued_param_send(gcsmav);
        break;

    case MAV_MSG_HEARTBEAT:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, HEARTBEAT);
        gcsmav->last_heartbeat_time = time_millis();
        gcsmav_send_heartbeat(gcsmav);
        break;

    case MAV_MSG_HWSTATUS:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, HWSTATUS);
        gcsmav_send_hwstatus(gcsmav);
        break;

    case MAV_MSG_LOCATION:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, GLOBAL_POSITION_INT);
        gcsmav_send_global_position_int(gcsmav);
        break;

    case MAV_MSG_HOME:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, HOME_POSITION);
        gcsmav_send_home_position(gcsmav);
        break;

    case MAV_MSG_ORIGIN:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, GPS_GLOBAL_ORIGIN);
        gcsmav_send_gps_global_origin(gcsmav);
        break;

    case MAV_MSG_CURRENT_WAYPOINT:
    case MAV_MSG_MISSION_ITEM_REACHED:
        ret = gcsmav_try_send_mission_message(gcsmav, id);
        break;

    case MAV_MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
    case MAV_MSG_NEXT_MISSION_REQUEST_RALLY:
    case MAV_MSG_NEXT_MISSION_REQUEST_FENCE:
        break;

    case MAV_MSG_MAG_CAL_PROGRESS:
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        ret = sensor_compass_send_mag_cal_progress(gcsmav);
#endif
        break;
    case MAV_MSG_MAG_CAL_REPORT:
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
        ret = sensor_compass_send_mag_cal_report(gcsmav);
#endif
        break;

    case MAV_MSG_BATTERY_STATUS:
        gcsmav_send_battery_status(gcsmav);
        break;

#if MB_MAVLINK_BATTERY2_ENABLED
    case MAV_MSG_BATTERY2:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, BATTERY2);
        send_battery2();
        break;
#endif

    case MAV_MSG_EKF_STATUS_REPORT:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, EKF_STATUS_REPORT);
        gcsmav_send_status_report(gcsmav);
        break;

    case MAV_MSG_MEMINFO:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, MEMINFO);
        gcsmav_send_meminfo(gcsmav);
        break;

    case MAV_MSG_RANGEFINDER:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, RANGEFINDER);
        gcsmav_send_rangefinder(gcsmav);
        break;

    case MAV_MSG_SYSTEM_TIME:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SYSTEM_TIME);
        gcsmav_send_system_time(gcsmav);
        break;
    case MAV_MSG_GPS_RAW:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, GPS_RAW_INT);
        gcsmav_send_mavlink_gps_raw(gcsmav);
        break;

    case MAV_MSG_GPS2_RAW:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, GPS2_RAW);
        gcsmav_send_mavlink_gps2_raw(gcsmav);
        break;

#if 0
    case MAV_MSG_GPS_RTK:
        CHECK_PAYLOAD_SIZE(GPS_RTK);
        AP::gps().send_mavlink_gps_rtk(chan, 0);
        break;

    case MAV_MSG_GPS2_RTK:
#if GPS_MAX_RECEIVERS > 1
        CHECK_PAYLOAD_SIZE(GPS2_RTK);
        AP::gps().send_mavlink_gps_rtk(chan, 1);
#endif
        break;
#endif

    case MAV_MSG_LOCAL_POSITION:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, LOCAL_POSITION_NED);
        gcsmav_send_local_position(gcsmav);
        break;

    case MAV_MSG_ATTITUDE_TARGET:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, ATTITUDE_TARGET);
        gcsmav_send_attitude_target(gcsmav);
        break;

    case MAV_MSG_POSITION_TARGET_GLOBAL_INT:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, POSITION_TARGET_GLOBAL_INT);
        gcsmav_send_position_target_global_int(gcsmav);
        break;

    case MAV_MSG_POSITION_TARGET_LOCAL_NED:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, POSITION_TARGET_LOCAL_NED);
        gcsmav_send_position_target_local_ned(gcsmav);
        break;

    case MAV_MSG_POWER_STATUS:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, POWER_STATUS);
        gcsmav_send_power_status(gcsmav);
        break;

    case MAV_MSG_MCU_STATUS:
#if defined(HAL_WITH_MCU_MONITORING)
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, MCU_STATUS);
        gcsmav_send_mcu_status(gcsmav);
#endif
        break;

    case MAV_MSG_RC_CHANNELS:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, RC_CHANNELS);
        gcsmav_send_rc_channels(gcsmav);
        break;

    case MAV_MSG_RC_CHANNELS_RAW:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, RC_CHANNELS_RAW);
        gcsmav_send_rc_channels_raw(gcsmav);
        break;

    case MAV_MSG_RAW_IMU:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, RAW_IMU);
        gcsmav_send_raw_imu(gcsmav);
        break;

    case MAV_MSG_SCALED_IMU:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_IMU);
        gcsmav_send_scaled_imu(gcsmav, 0, mavlink_msg_scaled_imu_send);
        break;

    case MAV_MSG_SCALED_IMU2:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_IMU2);
        gcsmav_send_scaled_imu(gcsmav, 1, mavlink_msg_scaled_imu2_send);
        break;

    case MAV_MSG_SCALED_IMU3:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_IMU3);
        gcsmav_send_scaled_imu(gcsmav, 2, mavlink_msg_scaled_imu3_send);
        break;

    case MAV_MSG_SCALED_PRESSURE:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_PRESSURE);
        gcsmav_send_scaled_pressure(gcsmav);
        break;

    case MAV_MSG_SCALED_PRESSURE2:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_PRESSURE2);
        gcsmav_send_scaled_pressure2(gcsmav);
        break;

    case MAV_MSG_SCALED_PRESSURE3:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SCALED_PRESSURE3);
        gcsmav_send_scaled_pressure3(gcsmav);
        break;

    case MAV_MSG_SERVO_OUTPUT_RAW:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SERVO_OUTPUT_RAW);
        gcsmav_send_servo_output_raw(gcsmav);
        break;

#if 0
    case MSG_SIMSTATE:
#if AP_SIM_ENABLED
        CHECK_PAYLOAD_SIZE(SIMSTATE);
        send_simstate();
#endif
        break;

    case MSG_SIM_STATE:
#if AP_SIM_ENABLED
        CHECK_PAYLOAD_SIZE(SIM_STATE);
        send_sim_state();
#endif
        break;
#endif

    case MAV_MSG_SYS_STATUS:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, SYS_STATUS);
        gcsmav_send_sys_status(gcsmav);
        break;

#if 0
    case MAV_MSG_AHRS2:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, AHRS2);
        gcsmav_send_ahrs2(gcsmav);
        break;
#endif

    case MAV_MSG_PID_TUNING:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, PID_TUNING);
        gcsmav_send_pid_tuning(gcsmav);
        break;

    case MAV_MSG_NAV_CONTROLLER_OUTPUT:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, NAV_CONTROLLER_OUTPUT);
        gcsmav_send_nav_controller_output(gcsmav);
        break;

#if 0
    case MAV_MSG_AHRS:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, AHRS);
        gcsmav_send_ahrs();
        break;
#endif

    case MAV_MSG_EXTENDED_SYS_STATE:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, EXTENDED_SYS_STATE);
        gcsmav_send_extended_sys_state(gcsmav);
        break;

    case MAV_MSG_VFR_HUD:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, VFR_HUD);
        gcsmav_send_vfr_hud(gcsmav);
        break;

    case MAV_MSG_VIBRATION:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, VIBRATION);
        gcsmav_send_vibration(gcsmav);
        break;

    case MAV_MSG_AUTOPILOT_VERSION:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, AUTOPILOT_VERSION);
        gcsmav_send_autopilot_version(gcsmav);
        break;

    case MAV_MSG_HIGH_LATENCY2:
#if HAL_HIGH_LATENCY2_ENABLED
        //MAV_CHECK_PAYLOAD_SIZE(HIGH_LATENCY2);
        //gcsmav_send_high_latency2();
#endif // HAL_HIGH_LATENCY2_ENABLED
        break;

    case MAV_MSG_GPS_RTK:
    case MAV_MSG_GPS2_RTK:
    case MAV_MSG_SERVO_OUT:
    case MAV_MSG_FENCE_STATUS:
    case MAV_MSG_AHRS:
    case MAV_MSG_SIMSTATE:
    case MAV_MSG_SIM_STATE:
    case MAV_MSG_AHRS2:
    case MAV_MSG_WIND:
    case MAV_MSG_DISTANCE_SENSOR:
    case MAV_MSG_TERRAIN:
    case MAV_MSG_CAMERA_FEEDBACK:
    case MAV_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS:
    case MAV_MSG_OPTICAL_FLOW:
    case MAV_MSG_RPM:
    case MAV_MSG_WHEEL_DISTANCE:
    case MAV_MSG_ADSB_VEHICLE:
    case MAV_MSG_AOA_SSA:
    case MAV_MSG_LANDING:
    case MAV_MSG_ESC_TELEMETRY:
    case MAV_MSG_NAMED_FLOAT:
    case MAV_MSG_EFI_STATUS:
    case MAV_MSG_GENERATOR_STATUS:
    case MAV_MSG_WINCH_STATUS:
    case MAV_MSG_WATER_DEPTH:
    case MAV_MSG_AIS_VESSEL:
    case MAV_MSG_UAVIONIX_ADSB_OUT_STATUS:
    case MAV_MSG_HYGROMETER:
    case MAV_MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE:
        break;

    default:
        // try_send_message must always at some stage return true for
        // a message, or we will attempt to infinitely retry the
        // message as part of send_message.
        // This message will be sent out at the same rate as the
        // unknown message, so should be safe.
        gcs_send_text(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        console_panic("Sending unknown mb_message %u", id);
#endif
        break;
    }

    return ret;
}

enum mb_message gcsmav_next_deferred_bucket_message_to_send(gcs_mavlink_t gcsmav, uint16_t now16_ms)
{
    if (gcsmav->sending_bucket_id == gcs_no_bucket_to_send) {
        // could happen if all streamrates are zero?
        return gcs_no_message_to_send;
    }

    const uint16_t ms_since_last_sent = now16_ms - gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id].last_sent_ms;
    if (ms_since_last_sent < gcsmav_get_reschedule_interval_ms(gcsmav, &gcsmav->deferred_message_bucket[gcsmav->sending_bucket_id])) {
        // not time to send this bucket
        return gcs_no_message_to_send;
    }

    const int16_t next = mb_bitmask_first_set(&gcsmav->bucket_message_ids_to_send);
    if (next == -1) {
        // should not happen
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
        console_panic("next_deferred_bucket_message_to_send called on empty bucket");
#endif
        gcsmav_find_next_bucket_to_send(gcsmav, now16_ms);
        return gcs_no_message_to_send;
    }
    return (enum mb_message)next;
}

/*
  return true if we will accept this packet. Used to implement SYSID_ENFORCE
 */
bool gcsmav_accept_packet(gcs_mavlink_t gcsmav, const mavlink_status_t *status, const mavlink_message_t *msg)
{
    if (msg->sysid == mavlink_system.sysid) {
        // accept packets from our own components
        // (e.g. mavlink-connected companion computers)
        return true;
    }

    if (msg->sysid == gcsmav_sysid_my_gcs(gcsmav)) {
        return true;
    }

    if (msg->msgid == MAVLINK_MSG_ID_RADIO ||
        msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
        return true;
    }

    if (!gcsmav_sysid_enforce(gcsmav)) {
        return true;
    }

    return false;
}

MAV_RESULT gcsmav_handle_command_set_message_interval(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet)
{
    return gcsmav_set_message_interval(gcsmav, (uint32_t)packet->param1, (int32_t)packet->param2);
}

MAV_RESULT gcsmav_set_message_interval(gcs_mavlink_t gcsmav, uint32_t msg_id, int32_t interval_us)
{
    const enum mb_message id = gcsmav_mavlink_id_to_ap_message_id(msg_id);
    if (id == MAV_MSG_LAST) {
        gcs_send_text(MAV_SEVERITY_INFO, "No mb_message for mavlink id (%u)", (unsigned int)msg_id);
        return MAV_RESULT_DENIED;
    }

    uint16_t interval_ms;
    if (interval_us == 0) {
        // zero is "reset to default rate"
        if (!gcsmav_get_default_interval_for_mb_message(gcsmav, id, &interval_ms)) {
            // if we don't have a default interval then we assume that
            // we do not send that message by default.  That may not
            // be strictly true if some random piece of code has set a
            // rate as part of its initialisation - in which case that
            // piece of code should probably be adding something into
            // whatever get_default_interval_for_ap_message is looking
            // at.
            interval_ms = 0;
        }
    } else if (interval_us == -1) {
        // minus-one is "stop sending"
        interval_ms = 0;
    } else if (interval_us < 1000) {
        // don't squash sub-ms times to zero
        interval_ms = 1;
    } else if (interval_us > 60000000) {
        interval_ms = 60000;
    } else {
        interval_ms = interval_us / 1000;
    }
    if (gcsmav_set_mb_message_interval(gcsmav, id, interval_ms)) {
        return MAV_RESULT_ACCEPTED;
    }

    return MAV_RESULT_FAILED;
}

MAV_RESULT gcsmav_handle_command_get_message_interval(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet)
{
    if (gcsmav_txspace(gcsmav) < MAV_PAYLOAD_SIZE(gcsmav->chan, MESSAGE_INTERVAL) +  MAV_PAYLOAD_SIZE(gcsmav->chan, COMMAND_ACK)) {
        return MAV_RESULT_TEMPORARILY_REJECTED;
    }

    const uint32_t mavlink_id = (uint32_t)packet->param1;
    if (mavlink_id >= 2 << 15) {
        // response packet limits range this works against!
        mavlink_msg_message_interval_send(gcsmav->chan, mavlink_id, 0); // not available
        return MAV_RESULT_FAILED;
    }

    const enum mb_message id = gcsmav_mavlink_id_to_ap_message_id(mavlink_id);
    if (id == MAV_MSG_LAST) {
        mavlink_msg_message_interval_send(gcsmav->chan, mavlink_id, 0); // not available
        return MAV_RESULT_FAILED;
    }

    uint16_t interval_ms = 0;
    if (!gcsmav_get_mb_message_interval(gcsmav, id, &interval_ms)) {
        // not streaming this message at the moment...
        mavlink_msg_message_interval_send(gcsmav->chan, mavlink_id, -1); // disabled
        return MAV_RESULT_ACCEPTED;
    }

    if (interval_ms == 0) {
        mavlink_msg_message_interval_send(gcsmav->chan, mavlink_id, -1); // disabled
        return MAV_RESULT_ACCEPTED;
    }

    mavlink_msg_message_interval_send(gcsmav->chan, mavlink_id, interval_ms * 1000);
    return MAV_RESULT_ACCEPTED;
}

bool gcsmav_get_mb_message_interval(gcs_mavlink_t gcsmav, enum mb_message  id, uint16_t *interval_ms)
{
    // check if it's a specially-handled message:
    const int8_t deferred_offset = gcsmav_get_deferred_message_index(gcsmav, id);
    if (deferred_offset != -1) {
        *interval_ms = gcsmav->deferred_message[deferred_offset].interval_ms;
        return true;
    }

    // check the deferred message buckets:
    for (uint8_t i=0; i<ARRAY_SIZE(gcsmav->deferred_message_bucket); i++) {
        const struct deferred_message_bucket_t *bucket = &gcsmav->deferred_message_bucket[i];
        if (mb_bitmask_get(&bucket->mb_message_ids, id)) {
            *interval_ms = bucket->interval_ms;
            return true;
        }
    }

    return false;
}

MAV_RESULT gcsmav_handle_command_request_message(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet)
{
    const uint32_t mavlink_id = (uint32_t)packet->param1;
    const enum mb_message id = gcsmav_mavlink_id_to_ap_message_id(mavlink_id);
    if (id == MAV_MSG_LAST) {
        return MAV_RESULT_FAILED;
    }

    gcsmav_send_message(gcsmav, id);
    return MAV_RESULT_ACCEPTED;
}

void gcsmav_handle_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    uitc_mavlink_message mavlink_message;

    mavlink_message.timestamp_us = time_micros64();
    mavlink_message.chan = gcsmav->chan;
    mavlink_message.num_gcs = gcs_num_gcs();
    mavlink_message.msg_t = (const void *)msg;

    switch ((uint8_t)gcsmav->chan) {
        case 0: {
            itc_publish(ITC_ID(mavlink_message), &mavlink_message);
            break;
        }

        case 1: {
            itc_publish(ITC_ID(mavlink_message1), &mavlink_message);
            break;
        }

        case 2: {
            itc_publish(ITC_ID(mavlink_message2), &mavlink_message);
            break;
        }

        case 3: {
            itc_publish(ITC_ID(mavlink_message3), &mavlink_message);
            break;
        }

        case 4: {
            itc_publish(ITC_ID(mavlink_message4), &mavlink_message);
            break;
        }

        case 5: {
            itc_publish(ITC_ID(mavlink_message5), &mavlink_message);
            break;
        }

        case 6: {
            itc_publish(ITC_ID(mavlink_message6), &mavlink_message);
            break;
        }

        default:
            break;
    }

    gcsmav_handle_common_message(gcsmav, msg);
}

/*
  handle messages which don't require vehicle specific data
 */
void gcsmav_handle_common_message(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    switch (msg->msgid) {

    case MAVLINK_MSG_ID_HEARTBEAT: {
        gcsmav_handle_heartbeat(gcsmav, msg);
        break;
    }

    case MAVLINK_MSG_ID_SETUP_SIGNING:
        gcsmav_handle_setup_signing(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
    case MAVLINK_MSG_ID_PARAM_SET:
    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
        gcsmav_handle_common_param_message(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
        gcsmav_handle_set_gps_global_origin(gcsmav, msg);
        break;


    case MAVLINK_MSG_ID_TIMESYNC:
        gcsmav_handle_timesync(gcsmav, msg);
        break;

#if MB_GCS_FTP_ENABLED
    case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
        gcsmav_handle_file_transfer_protocol(gcsmav, msg);
        break;
#endif

    case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
        gcsmav_handle_send_autopilot_version(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_COMMAND_LONG:
        gcsmav_handle_command_long(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_LED_CONTROL:
        // send message to Notify
        notify_handle_led_control(msg);
        break;

    case MAVLINK_MSG_ID_PLAY_TUNE:
        // send message to Notify
        notify_handle_play_tune(msg);
        break;

    case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
        // only pass if override is not selected 
        gcsmav_handle_request_data_stream(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_SYSTEM_TIME:
        gcsmav_handle_system_time_message(msg);
        break;

    case MAVLINK_MSG_ID_LANDING_TARGET:
        gcsmav_handle_landing_target2(gcsmav, msg);
        break;

    case MAVLINK_MSG_ID_RADIO:
    case MAVLINK_MSG_ID_RADIO_STATUS:       // MAV ID: 109
    {
        gcsmav_handle_radio_status(gcsmav, msg);
        break;
    }
    }
}

void gcsmav_handle_heartbeat(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    // if the heartbeat is from our GCS then we don't failsafe for
    // now...
    if (msg->sysid == gcsmav_sysid_my_gcs(gcsmav)) {
        gcs_sysid_myggcs_seen(time_millis());
    }
}

/*
  return a timesync request
  Sends back ts1 as received, and tc1 is the local timestamp in usec
 */
void gcsmav_handle_timesync(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    // decode incoming timesync message
    mavlink_timesync_t tsync;
    mavlink_msg_timesync_decode(msg, &tsync);

    if (tsync.tc1 != 0) {
        // this is a response to a timesync request
        if (tsync.ts1 != gcsmav->_timesync_request.sent_ts1) {
            // we didn't actually send the request.... or it's a
            // response to an ancient request...
            return;
        }
        const uint64_t round_trip_time_us = (gcsmav_timesync_receive_timestamp_ns(gcsmav) - gcsmav->_timesync_request.sent_ts1)*0.001f;
#if 0
        gcs_send_text(MAV_SEVERITY_INFO,
                        "timesync response sysid=%u (latency=%fms)",
                        msg->sysid,
                        round_trip_time_us*0.001f);
        AP_Logger *logger = AP_Logger::get_singleton();
        if (logger != nullptr) {
            AP::logger().Write(
                "TSYN",
                "TimeUS,SysID,RTT",
                "s-s",
                "F-F",
                "QBQ",
                AP_HAL::micros64(),
                msg.sysid,
                round_trip_time_us
                );
        }
#endif

        return;
    }

    if (!MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, TIMESYNC)) {
        // drop this timesync request entirely
        return;
    }

    // create new timesync struct with tc1 field as system time in
    // nanoseconds.  The client timestamp is as close as possible to
    // the time we received the TIMESYNC message.
    mavlink_timesync_t rsync;
    rsync.tc1 = gcsmav_timesync_receive_timestamp_ns(gcsmav);
    rsync.ts1 = tsync.ts1;

    // respond with a timesync message
    mavlink_msg_timesync_send(
        gcsmav->chan,
        rsync.tc1,
        rsync.ts1,
        0,
        0);
}

void gcsmav_handle_send_autopilot_version(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    gcsmav_send_message(gcsmav, MAV_MSG_AUTOPILOT_VERSION);
}

void gcsmav_handle_system_time_message(const mavlink_message_t *msg)
{
    mavlink_system_time_t packet;
    mavlink_msg_system_time_decode(msg, &packet);

    rtc_set_utc_usec(packet.time_unix_usec, SOURCE_MAVLINK_SYSTEM_TIME);
}

void gcsmav_handle_landing_target2(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_landing_target_t m;
    mavlink_msg_landing_target_decode(msg, &m);

    // correct offboard timestamp
    const uint32_t corrected_ms = gcsmav_correct_offboard_timestamp_usec_to_ms(gcsmav, m.time_usec, MAV_PAYLOAD_SIZE(gcsmav->chan, LANDING_TARGET));
    gcsmav_handle_landing_target(gcsmav, &m, corrected_ms);
}

/*
  correct an offboard timestamp in microseconds into a local timestamp
  since boot in milliseconds. See the JitterCorrection code for details

  Return a value in milliseconds since boot (for use by the EKF)
 */
uint32_t gcsmav_correct_offboard_timestamp_usec_to_ms(gcs_mavlink_t gcsmav, uint64_t offboard_usec, uint16_t payload_size)
{
    uint64_t local_us;
    // if the HAL supports it then constrain the latest possible time
    // the packet could have been sent by the uart receive time and
    // the baudrate and packet size.
    uint64_t uart_receive_time = SerialManager_receive_time_constraint_us(gcsmav->_port, payload_size);
    if (uart_receive_time != 0) {
        local_us = uart_receive_time;
    } else {
        local_us = time_micros64();
    }
    uint64_t corrected_us = jitter_correct_offboard_timestamp_usec(&gcsmav->lag_correction , offboard_usec, local_us);

    return corrected_us / 1000U;
}

void gcsmav_handle_set_gps_global_origin(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_set_gps_global_origin_t packet;
    mavlink_msg_set_gps_global_origin_decode(msg, &packet);

    // sanity check location
    if (!location_check_lat_lng2(packet.latitude, packet.longitude)) {
        // silently drop the request
        return;
    }

    Location ekf_origin = {0};
    ekf_origin.lat = packet.latitude;
    ekf_origin.lng = packet.longitude;
    ekf_origin.alt = packet.altitude / 10;
    gcsmav_set_ekf_origin(gcsmav, &ekf_origin);
}

// sets ekf_origin if it has not been set.
//  should only be used when there is no GPS to provide an absolute position
void gcsmav_set_ekf_origin(gcs_mavlink_t gcsmav, const Location* loc)
{
    // check location is valid
    if (!location_check_lat_lng(loc)) {
        return;
    }

    ahrs_view* ahrs = get_ahrs_view();

    // check if EKF origin has already been set
    Location ekf_origin;
    if (ahrs_get_origin(ahrs, &ekf_origin)) {
        return;
    }

#if 0
    if (!ahrs_set_origin(loc)) {
        return;
    }
#endif

    //ahrs.Log_Write_Home_And_Origin();

    // send ekf origin to GCS
    if (!gcsmav_try_send_message(gcsmav, MAV_MSG_ORIGIN)) {
        // try again later
        gcsmav_send_message(gcsmav, MAV_MSG_ORIGIN);
    }
}

void gcsmav_handle_command_long(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    // decode packet
    mavlink_command_long_t packet;
    mavlink_msg_command_long_decode(msg, &packet);

    //hal.util->persistent_data.last_mavlink_cmd = packet.command;

    MAV_RESULT result;

    // special handling of messages that need the mavlink_message_t
    switch (packet.command) {
    case MAV_CMD_SET_MESSAGE_INTERVAL:
        result = gcsmav_handle_command_set_message_interval(gcsmav, &packet);
        break;

    case MAV_CMD_GET_MESSAGE_INTERVAL:
        result = gcsmav_handle_command_get_message_interval(gcsmav, &packet);
        break;

    case MAV_CMD_REQUEST_MESSAGE:
        result = gcsmav_handle_command_request_message(gcsmav, &packet);
        break;

    case MAV_CMD_GET_HOME_POSITION:
        result = gcsmav_handle_command_get_home_position(gcsmav, &packet);
        break;

    default:
         result = MAV_RESULT_ENUM_END;
         break;
    }

    if (result == MAV_RESULT_ENUM_END) {
        return;
    }

    // send ACK or NAK
    mavlink_msg_command_ack_send(gcsmav->chan, packet.command, result,
                                 0, 0,
                                 msg->sysid,
                                 msg->compid);

#if 0
    // log the packet:
    mavlink_command_int_t packet_int;
    convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int);
    AP::logger().Write_Command(packet_int, msg.sysid, msg.compid, result, true);

    hal.util->persistent_data.last_mavlink_cmd = 0;
#endif
}

void gcsmav_handle_radio_status(gcs_mavlink_t gcsmav, const mavlink_message_t *msg)
{
    mavlink_radio_t packet;
    mavlink_msg_radio_decode(msg, &packet);

    const uint32_t now = time_millis();

    gcs_last_radio_status.received_ms = now;
    gcs_last_radio_status.rssi = packet.rssi;

    // record if the GCS has been receiving radio messages from
    // the aircraft
    if (packet.remrssi != 0) {
        gcs_last_radio_status.remrssi_ms = now;
    }

    gcsmav->last_txbuf = packet.txbuf;

    // use the state of the transmit buffer in the radio to
    // control the stream rate, giving us adaptive software
    // flow control
    if (packet.txbuf < 20 && gcsmav->stream_slowdown_ms < 2000) {
        // we are very low on space - slow down a lot
        gcsmav->stream_slowdown_ms += 60;
    } else if (packet.txbuf < 50 && gcsmav->stream_slowdown_ms < 2000) {
        // we are a bit low on space, slow down slightly
        gcsmav->stream_slowdown_ms += 20;
    } else if (packet.txbuf > 95 && gcsmav->stream_slowdown_ms > 200) {
        // the buffer has plenty of space, speed up a lot
        gcsmav->stream_slowdown_ms -= 40;
    } else if (packet.txbuf > 90 && gcsmav->stream_slowdown_ms != 0) {
        // the buffer has enough space, speed up a bit
        gcsmav->stream_slowdown_ms -= 20;
    }

#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
    if (gcsmav->stream_slowdown_ms > gcsmav->max_slowdown_ms) {
        gcsmav->max_slowdown_ms = gcsmav->stream_slowdown_ms;
    }
#endif

#if 0
    //log rssi, noise, etc if logging Performance monitoring data
    if (log_radio) {
        AP::logger().Write_Radio(packet);
    }
#endif
}

MAV_RESULT gcsmav_handle_command_get_home_position(gcs_mavlink_t gcsmav, const mavlink_command_long_t *packet)
{
    if (!ahrs_home_is_set(get_ahrs_view())) {
        return MAV_RESULT_FAILED;
    }
    if (!gcsmav_try_send_message(gcsmav, MAV_MSG_HOME)) {
        // try again later
        gcsmav_send_message(gcsmav, MAV_MSG_HOME);
    }
    if (!gcsmav_try_send_message(gcsmav, MAV_MSG_ORIGIN)) {
        // try again later
        gcsmav_send_message(gcsmav, MAV_MSG_ORIGIN);
    }

    return MAV_RESULT_ACCEPTED;
}


/// send msg
/*
 * broadcast a timesync message.  We may get multiple responses to this request.
 */
void gcsmav_send_timesync(gcs_mavlink_t gcsmav)
{
    gcsmav->_timesync_request.sent_ts1 = gcsmav_timesync_timestamp_ns(gcsmav);
    mavlink_msg_timesync_send(
        gcsmav->chan,
        0,
        gcsmav->_timesync_request.sent_ts1,
        0,
        0
        );
}

uint64_t gcsmav_timesync_receive_timestamp_ns(gcs_mavlink_t gcsmav)
{
    uint64_t ret = SerialManager_receive_time_constraint_us(gcsmav->_port, MAV_PAYLOAD_SIZE(gcsmav->chan, TIMESYNC));
    if (ret == 0) {
        ret = time_micros64();
    }
    return ret*1000LL;
}

uint64_t gcsmav_timesync_timestamp_ns(gcs_mavlink_t gcsmav)
{
    // we add in our own system id try to ensure we only consider
    // responses to our own timesync request messages
    return time_micros64()*1000LL + mavlink_system.sysid;
}

// send data for barometer and airspeed sensors instances.  In the
// case that we run out of instances of one before the other we send
// the relevant fields as 0.
static void gcsmav_send_scaled_pressure_instance(gcs_mavlink_t gcsmav, uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff))
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    sensor_baro* barometer = sensor_baro_get_singleton();


    bool have_data = false;

    float press_abs = 0.0f;
    int16_t temperature = 0; // Absolute pressure temperature
    int16_t temperature_press_diff = 0; // Differential pressure temperature
    if (instance < barometer->_num_sensors) {
        press_abs = sensor_baro_get_pressure2(instance) * 0.01f;
        temperature = sensor_baro_get_temperature2(instance)*100;
        have_data = true;
    }

    float press_diff = 0; // pascal
#if AP_AIRSPEED_ENABLED
    AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
    if (airspeed != nullptr &&
        airspeed->enabled(instance)) {
        press_diff = airspeed->get_differential_pressure(instance) * 0.01f;
        float temp;
        if (airspeed->get_temperature(instance,temp)) {
            temperature_press_diff = temp * 100;
            if (temperature_press_diff == 0) {
                // don't send zero as that is the value for 'no data'
                temperature_press_diff = 1;
            }
        }
        have_data = true;
    }
#endif

    if (!have_data) {
        return;
    }

    send_fn(
        gcsmav->chan,
        time_millis(),
        press_abs, // hectopascal
        press_diff, // hectopascal
        temperature, // 0.01 degrees C
        temperature_press_diff); // 0.01 degrees C
#else
    (void)gcsmav;
    (void)instance;
    (void)send_fn;
#endif
}

void gcsmav_send_scaled_pressure(gcs_mavlink_t gcsmav)
{
    gcsmav_send_scaled_pressure_instance(gcsmav, 0, mavlink_msg_scaled_pressure_send);
}

void gcsmav_send_scaled_pressure2(gcs_mavlink_t gcsmav)
{
    gcsmav_send_scaled_pressure_instance(gcsmav, 1, mavlink_msg_scaled_pressure2_send);
}

void gcsmav_SendScaledPressure3(gcs_mavlink_t gcsmav)
{
    gcsmav_send_scaled_pressure_instance(gcsmav, 2, mavlink_msg_scaled_pressure3_send);
}

void gcsmav_send_attitude(gcs_mavlink_t gcsmav)
{
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;

    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);

    mavlink_msg_attitude_send(
        gcsmav->chan,
        time_millis(),
        vehicle_attitude.vehicle_euler.roll,
        vehicle_attitude.vehicle_euler.pitch,
        vehicle_attitude.vehicle_euler.yaw,
        sensor_gyr.sensor_gyr_filter[0],
        sensor_gyr.sensor_gyr_filter[1],
        sensor_gyr.sensor_gyr_filter[2]);
}

void gcsmav_send_attitude_quaternion(gcs_mavlink_t gcsmav)
{
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;

    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);

    const float repr_offseq_q[] = {0,0,0,0};  // unused, but probably should correspond to the AHRS view?

    mavlink_msg_attitude_quaternion_send(
        gcsmav->chan,
        time_millis(),
        vehicle_attitude.vehicle_quat.q0,
        vehicle_attitude.vehicle_quat.q1,
        vehicle_attitude.vehicle_quat.q2,
        vehicle_attitude.vehicle_quat.q3,
        sensor_gyr.sensor_gyr_filter[0], // rollspeed
        sensor_gyr.sensor_gyr_filter[1], // pitchspeed
        sensor_gyr.sensor_gyr_filter[2], // yawspeed
        repr_offseq_q
        );
}

uint64_t gcsmavCapabilities(gcs_mavlink_t gcsmav)
{
    uint64_t ret = MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
        MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION;

    const enum SerialProtocol mavlink_protocol = SerialManager_get_protocol(gcsmav->_port);
    if (mavlink_protocol == SerialProtocol_MAVLink2 || mavlink_protocol == SerialProtocol_MAVLinkHL) {
        ret |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
    }

#if 0
    AP_AdvancedFailsafe *failsafe = AP::advancedfailsafe();
    if (failsafe != nullptr && failsafe->enabled()) {
        // Copter and Sub may also set this bit as they can always terminate
        ret |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
    }

#if HAL_RALLY_ENABLED
    if (AP::rally()) {
        ret |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
    }
#endif

#if AP_FENCE_ENABLED
    if (AP::fence()) {
        ret |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
    }
#endif
#endif

#if MB_GCS_FTP_ENABLED
    if (!brd_ftp_disabled()){  //if ftp disable board option is not set
        ret |= MAV_PROTOCOL_CAPABILITY_FTP;
    }
#endif

    return ret;
}

void gcsmav_SendBanner()
{
    // mark the firmware version in the tlog
    gcs_send_text(MAV_SEVERITY_INFO, "%s", THISFIRMWARE);

#if 0
    // mark the firmware version in the tlog
    const AP_FWVersion &fwver = AP::fwversion();

    send_text(MAV_SEVERITY_INFO, "%s", fwver.fw_string);

    if (fwver.middleware_name && fwver.os_name) {
        send_text(MAV_SEVERITY_INFO, "%s: %s %s: %s",
                  fwver.middleware_name, fwver.middleware_hash_str,
                  fwver.os_name, fwver.os_hash_str);
    } else if (fwver.os_name) {
        send_text(MAV_SEVERITY_INFO, "%s: %s",
                  fwver.os_name, fwver.os_hash_str);
    }

    // send system ID if we can
    char sysid[50];
    if (hal.util->get_system_id(sysid)) {
        send_text(MAV_SEVERITY_INFO, "%s", sysid);
    }

    // send RC output mode info if available
    char banner_msg[50];
    if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) {
        send_text(MAV_SEVERITY_INFO, "%s", banner_msg);
    }

#if HAL_INS_ENABLED
    // output any fast sampling status messages
    for (uint8_t i = 0; i < INS_MAX_BACKENDS; i++) {
        if (AP::ins().get_output_banner(i, banner_msg, sizeof(banner_msg))) {
            send_text(MAV_SEVERITY_INFO, "%s", banner_msg);
        }
    }
#endif
#endif
}

void gcsmav_send_global_position_int(gcs_mavlink_t gcsmav)
{
    ahrs_view* ahrs = get_ahrs_view();

    (void)ahrs_get_location(ahrs, &gcsmav->global_position_current_loc); // return value ignored; we send stale data

    const Vector3f_t* vel = ahrs_get_velocity_neu_cms(ahrs);

    mavlink_msg_global_position_int_send(
        gcsmav->chan,
        time_millis(),
        gcsmav->global_position_current_loc.lat, // in 1E7 degrees
        gcsmav->global_position_current_loc.lng, // in 1E7 degrees
        gcsmav->global_position_current_loc.alt * 10,       // millimeters above ground/sea level
        (int32_t)ahrs_get_relative_position_u_home(ahrs) * 10, // millimeters above home
        vel->x,                     // X speed cm/s (+ve North)
        vel->y,                     // Y speed cm/s (+ve East)
        -vel->z,                    // Z speed cm/s (+ve Down)
        ahrs->yaw_sensor_cd);                // compass heading in 1/100 degree
}

void gcsmav_send_vfr_hud(gcs_mavlink_t gcsmav)
{
    uitc_vehicle_vfr_hud vfr_hud;
    itc_copy_from_hub(ITC_ID(vehicle_vfr_hud), &vfr_hud);

    mavlink_msg_vfr_hud_send(
        gcsmav->chan,
        vfr_hud.airspeed,
        vfr_hud.groundspeed,
        vfr_hud.heading,
        vfr_hud.throttle,
        vfr_hud.alt,
        vfr_hud.climb);
}

/*
  Send MAVLink heartbeat
 */
void gcsmav_send_heartbeat(gcs_mavlink_t gcsmav)
{
    uitc_vehicle_hearbeat hearbeat;
    itc_copy_from_hub(ITC_ID(vehicle_hearbeat), &hearbeat);

    mavlink_msg_heartbeat_send(
        gcsmav->chan,
        hearbeat.type,
        hearbeat.autopilot,
        hearbeat.base_mode,
        hearbeat.custom_mode,
        hearbeat.system_status);
}

void gcsmav_send_hwstatus(gcs_mavlink_t gcsmav)
{
    mavlink_msg_hwstatus_send(
        gcsmav->chan,
        analogin_board_voltage()*1000,
        0);
}

void gcsmav_send_home_position(gcs_mavlink_t gcsmav)
{
    ahrs_view* ahrs = get_ahrs_view();

    if (!ahrs_home_is_set(ahrs)) {
        return;
    }

    Location home;
    ahrs_get_home(ahrs, &home);

   // get home position from origin
    Vector3f_t home_pos_neu_cm;
    if (!location_get_vector_from_origin_neu(&home, &home_pos_neu_cm)) {
        return;
    }

    const float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
    mavlink_msg_home_position_send(
        gcsmav->chan,
        home.lat,
        home.lng,
        home.alt * 10,
        home_pos_neu_cm.x * 0.01f, // position of home on x-axis in meters
        home_pos_neu_cm.y * 0.01f, // position of home on y-axis in meters
        home_pos_neu_cm.z * -0.01f, // position of home on z-axis in meters in NED frame
        q,
        0.0f, 0.0f, 0.0f,
        time_micros64());
}

void gcsmav_send_gps_global_origin(gcs_mavlink_t gcsmav)
{
    Location ekf_origin;
    if (!ahrs_get_origin(get_ahrs_view(), &ekf_origin)) {
        return;
    }

    mavlink_msg_gps_global_origin_send(
        gcsmav->chan,
        ekf_origin.lat,
        ekf_origin.lng,
        ekf_origin.alt * 10,
        time_micros64());
}

void gcsmav_send_meminfo(gcs_mavlink_t gcsmav)
{
#ifdef RT_USING_HEAP
    uint16_t __brkval = 0;
    rt_size_t total = 0, used = 0, max_used = 0;
    rt_memory_info(&total, &used, &max_used);
    mavlink_msg_meminfo_send(gcsmav->chan, __brkval, MIN(total - used, 0xFFFFU), total - used);
#else
    (void)gcsmav;
#endif
}

/*
  send the SYSTEM_TIME message
 */
void gcsmav_send_system_time(gcs_mavlink_t gcsmav)
{
    uint64_t time_unix = 0;
    rtc_get_utc_usec(&time_unix); // may fail, leaving time_unix at 0

    mavlink_msg_system_time_send(
        gcsmav->chan,
        time_unix,
        time_millis());
}

void gcsmav_send_mavlink_gps_raw(gcs_mavlink_t gcsmav)
{
    uitc_sensor_gps gps_pos_t;

    if(itc_copy_from_hub(ITC_ID(sensor_gps_raw1), &gps_pos_t) == 0){
        mavlink_msg_gps_raw_int_send(
        gcsmav->chan, 
        gps_pos_t.timestamp_us,
        gps_pos_t.fix_type,
        gps_pos_t.lat,        // in 1E7 degrees
        gps_pos_t.lon,        // in 1E7 degrees
        gps_pos_t.alt_msl, // in mm
        gps_pos_t.hdop,
        gps_pos_t.vdop,
        gps_pos_t.vel_m_s*100,  // cm/s
        degrees(gps_pos_t.cog_rad) * 100, // 1/100 degrees,
        gps_pos_t.num_sats,
        gps_pos_t.have_undulation?gps_pos_t.alt_msl*10-gps_pos_t.undulation*1000:gps_pos_t.alt_msl,   // Elipsoid height in mm
        gps_pos_t.have_horizontal_accuracy?gps_pos_t.horizontal_accuracy*1000:0,          // one-sigma standard deviation in mm
        gps_pos_t.have_vertical_accuracy?gps_pos_t.vertical_accuracy*1000:0,          // one-sigma standard deviation in mm
        gps_pos_t.have_speed_accuracy?gps_pos_t.speed_accuracy*1000:0,          // one-sigma standard deviation in mm/s
        0,                    // TODO one-sigma heading accuracy standard deviation
        gps_pos_t.have_gps_yaw?gps_pos_t.gps_yaw*100:0);
    }
}

void gcsmav_send_mavlink_gps2_raw(gcs_mavlink_t gcsmav)
{
    uitc_sensor_gps gps_pos_t;

    if(itc_copy_from_hub(ITC_ID(sensor_gps_raw2), &gps_pos_t) == 0){
        mavlink_msg_gps2_raw_send(
        gcsmav->chan,
        gps_pos_t.timestamp_us,
        gps_pos_t.fix_type,
        gps_pos_t.lat,
        gps_pos_t.lon,
        gps_pos_t.alt_msl * 10UL,
        gps_pos_t.hdop,
        gps_pos_t.vdop,
        gps_pos_t.vel_m_s*100,  // cm/s
        degrees(gps_pos_t.cog_rad) * 100, // 1/100 degrees,
        gps_pos_t.num_sats,
        gps_pos_t.rtk_num_sats,
        gps_pos_t.rtk_age_ms,
        (gps_pos_t.gps_yaw_configured && gps_pos_t.have_gps_yaw)?gps_pos_t.gps_yaw*100:0,
        gps_pos_t.have_undulation?gps_pos_t.alt_msl*10-gps_pos_t.undulation*1000:gps_pos_t.alt_msl,   // Elipsoid height in mm
        gps_pos_t.have_horizontal_accuracy?gps_pos_t.horizontal_accuracy*1000:0,          // one-sigma standard deviation in mm
        gps_pos_t.have_vertical_accuracy?gps_pos_t.vertical_accuracy*1000:0,          // one-sigma standard deviation in mm
        gps_pos_t.have_speed_accuracy?gps_pos_t.speed_accuracy*1000:0,          // one-sigma standard deviation in mm/s
        0);                    // TODO one-sigma heading accuracy standard deviation
    }
}

/*
  send LOCAL_POSITION_NED message
 */
void gcsmav_send_local_position(gcs_mavlink_t gcsmav)
{
    uitc_vehicle_local_position local_position;
    itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position);

    mavlink_msg_local_position_ned_send(
        gcsmav->chan,
        time_millis(),
        local_position.x,
        local_position.y,
        local_position.z,
        local_position.vx,
        local_position.vy,
        local_position.vz);
}

// report power supply status
void gcsmav_send_power_status(gcs_mavlink_t gcsmav)
{
    uitc_power_status board_power_status;
    if (itc_copy_from_hub(ITC_ID(power_status), &board_power_status) != 0) {
        return;
    }

    mavlink_msg_power_status_send(gcsmav->chan,
                                  board_power_status.board_voltage * 1000,
                                  board_power_status.servorail_voltage * 1000,
                                  board_power_status.power_status_flags);
}

/*
  send RC_CHANNELS messages
 */
void gcsmav_send_rc_channels(gcs_mavlink_t gcsmav)
{
    uitc_actuator_rc actuator_rc;
    itc_copy_from_hub(ITC_ID(vehicle_rc), &actuator_rc);

    mavlink_msg_rc_channels_send(
        gcsmav->chan,
        actuator_rc.timestamp_us*0.001f,
        actuator_rc.channel_count,
        actuator_rc.channels[0],
        actuator_rc.channels[1],
        actuator_rc.channels[2],
        actuator_rc.channels[3],
        actuator_rc.channels[4],
        actuator_rc.channels[5],
        actuator_rc.channels[6],
        actuator_rc.channels[7],
        actuator_rc.channels[8],
        actuator_rc.channels[9],
        actuator_rc.channels[10],
        actuator_rc.channels[11],
        actuator_rc.channels[12],
        actuator_rc.channels[13],
        actuator_rc.channels[14],
        actuator_rc.channels[15],
        actuator_rc.channels[16],
        actuator_rc.channels[17],
        actuator_rc.rssi);
}

bool gcsmav_sending_mavlink1(gcs_mavlink_t gcsmav)
{
    const mavlink_status_t *status = mavlink_get_channel_status(gcsmav->chan);
    if (status == NULL) {
        // should not happen
        return true;
    }
    return ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0);
}

void gcsmav_send_rc_channels_raw(gcs_mavlink_t gcsmav)
{
    // for mavlink1 send RC_CHANNELS_RAW, for compatibility with OSD
    // implementations
    if (!gcsmav_sending_mavlink1(gcsmav)) {
        return;
    }

    uitc_actuator_rc actuator_rc;
    itc_copy_from_hub(ITC_ID(vehicle_rc), &actuator_rc);

    mavlink_msg_rc_channels_raw_send(
        gcsmav->chan,
        time_millis(),
        0,
        actuator_rc.channels[0],
        actuator_rc.channels[1],
        actuator_rc.channels[2],
        actuator_rc.channels[3],
        actuator_rc.channels[4],
        actuator_rc.channels[5],
        actuator_rc.channels[6],
        actuator_rc.channels[7],
        actuator_rc.rssi);
}

void gcsmav_send_raw_imu(gcs_mavlink_t gcsmav)
{
    uitc_sensor_acc sensor_acc;
    uitc_sensor_gyr sensor_gyr;
    uitc_sensor_mag sensor_mag;

    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag);

    mavlink_msg_raw_imu_send(
        gcsmav->chan,
        sensor_acc.timestamp_us,
        sensor_acc.sensor_acc_filter[0] * 1000.0f / GRAVITY_MSS,
        sensor_acc.sensor_acc_filter[1] * 1000.0f / GRAVITY_MSS,
        sensor_acc.sensor_acc_filter[2] * 1000.0f / GRAVITY_MSS,
        sensor_gyr.sensor_gyr_filter[0] * 1000.0f,
        sensor_gyr.sensor_gyr_filter[1] * 1000.0f,
        sensor_gyr.sensor_gyr_filter[2] * 1000.0f,
        sensor_mag.sensor_mag_correct[0],
        sensor_mag.sensor_mag_correct[1],
        sensor_mag.sensor_mag_correct[2],
        0,  // we use SCALED_IMU and SCALED_IMU2 for other IMUs
        (int16_t)(sensor_acc.temperature *100));
}

void gcsmav_send_scaled_imu(gcs_mavlink_t gcsmav, uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature))
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    const sensor_imu* ins = sensor_imu_get_singleton();
    const sensor_compass* compass = sensor_compass_get_singleton();
    int16_t _temperature = 0;

    bool have_data = false;
    Vector3f_t accel = {0};
    if (MIN(INS_MAX_INSTANCES, ins->_accel_count) > instance) {
        accel = ins->_accel[instance];
        _temperature = ins->_temperature[instance]*100;
        have_data = true;
    }
    Vector3f_t gyro = {0};
    if (MIN(INS_MAX_INSTANCES, ins->_gyro_count) > instance) {
        gyro = ins->_gyro[instance];
        have_data = true;
    }
    Vector3f_t mag = {0};
    if (MIN(COMPASS_MAX_INSTANCES, compass->_compass_count) > instance) {
        mag = *(sensor_compass_get_field2(instance));
        have_data = true;
    }
    if (!have_data) {
        return;
    }
    send_fn(
        gcsmav->chan,
        time_millis(),
        accel.x * 1000.0f / GRAVITY_MSS,
        accel.y * 1000.0f / GRAVITY_MSS,
        accel.z * 1000.0f / GRAVITY_MSS,
        gyro.x * 1000.0f,
        gyro.y * 1000.0f,
        gyro.z * 1000.0f,
        mag.x,
        mag.y,
        mag.z,
        _temperature);
#endif
}

/*
  send SERVO_OUTPUT_RAW
 */
void gcsmav_send_servo_output_raw(gcs_mavlink_t gcsmav)
{
    const uint32_t enabled_mask = ~(srv_channels_get_output_channel_mask(k_GPIO));
    if (enabled_mask == 0) {
        return;
    }

    uint16_t values[NUM_SERVO_CHANNELS] = {0};
    srv_hal_read2(values, NUM_SERVO_CHANNELS);
    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
        if (values[i] == 65535) {
            values[i] = 0;
        }
    }
    if ((enabled_mask & 0xFFFF) != 0) {
        mavlink_msg_servo_output_raw_send(
                gcsmav->chan,
                time_micros(),
                0,     // port
                values[0],  values[1],  values[2],  values[3],
                values[4],  values[5],  values[6],  values[7],
                values[8],  values[9],  values[10], values[11],
                values[12], values[13], values[14], values[15]);
    }

#if NUM_SERVO_CHANNELS >= 17
    if ((enabled_mask & 0xFFFF0000) != 0) {
        mavlink_msg_servo_output_raw_send(
                gcsmav->chan,
                time_micros(),
                1,     // port
                values[16],  values[17],  values[18],  values[19],
                values[20],  values[21],  values[22],  values[23],
                values[24],  values[25],  values[26], values[27],
                values[28], values[29], values[30], values[31]);
    }
#endif
}

void gcsmav_send_sys_status(gcs_mavlink_t gcsmav)
{
    // send extended status only once vehicle has been initialised
    // to avoid unnecessary errors being reported to user
    if (!gcs_vehicle_initialised()) {
        return;
    }

    uitc_battery_status battery_status;
    itc_copy_from_hub(ITC_ID(battery_status), &battery_status);

    float battery_current;
    if (battery_status.all_healthy && battery_status.primary_has_current) {
        battery_current = math_constrain_float(battery_status.primary_current_amps * 100,-INT16_MAX,INT16_MAX);
    } else {
        battery_current = -1;
    }

    uint32_t control_sensors_present;
    uint32_t control_sensors_enabled;
    uint32_t control_sensors_health;

    gcs_get_sensor_status_flags(&control_sensors_present, &control_sensors_enabled, &control_sensors_health);

    mavlink_msg_sys_status_send(
        gcsmav->chan,
        control_sensors_present,
        control_sensors_enabled,
        control_sensors_health,
        0,
        battery_status.primary_gcs_voltage * 1000,  // mV
        battery_current,        // in 10mA units
        battery_status.primary_healthy ? MIN(battery_status.primary_remaining_pct, INT8_MAX) : -1,      // in %
        0,  // comm drops %,
        0,  // comm drops in pkts,
        0,
        0,
        0,  // errors3
        0,  // errors4
        0,
        0,
        0);
}

void gcsmav_send_extended_sys_state(gcs_mavlink_t gcsmav)
{
    mavlink_msg_extended_sys_state_send(gcsmav->chan, gcsmav_vtol_state(gcsmav), gcsmav_landed_state(gcsmav));
}

/*
  send VIBRATION message
 */
void gcsmav_send_vibration(gcs_mavlink_t gcsmav)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    Vector3f_t vibration = sensor_imu_get_vibration_levels();

    mavlink_msg_vibration_send(
        gcsmav->chan,
        time_micros64(),
        vibration.x,
        vibration.y,
        vibration.z,
        sensor_imu_get_accel_clip_count(0),
        sensor_imu_get_accel_clip_count(1),
        sensor_imu_get_accel_clip_count(2));
#endif
}

/*
  send AUTOPILOT_VERSION packet
 */
void gcsmav_send_autopilot_version(gcs_mavlink_t gcsmav)
{
    uint32_t flight_sw_version;
    uint32_t middleware_sw_version = 0;
    uint32_t board_version = 0;
    uint16_t vendor_id = 0;
    uint16_t product_id = 0;
    uint64_t uid = 0;
    uint8_t  uid2[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_UID2_LEN] = {0x00,0x48,0x00,0x3d,0x32,0x35,0x51,0x0f,0x31,0x36,0x33,0x39};

    uint8_t uid_len = sizeof(uid2); // taken as reference and modified
                                    // by following call:
    
    flight_sw_version = 4 << (8 * 3) | \
                        1 << (8 * 2) | \
                        0 << (8 * 1) | \
                        (uint32_t)(FIRMWARE_VERSION_TYPE_DEV) << (8 * 0);

    mavlink_autopilot_version_t ver = {
        .capabilities = gcsmav_capabilities(gcsmav),
        .flight_sw_version = flight_sw_version,
        .middleware_sw_version = middleware_sw_version,
        .os_sw_version = 0,
        .board_version = board_version,
        .flight_custom_version = "cb570c06",
        .middleware_custom_version = {0},
        .os_custom_version = "9d8e2b96",
        .vendor_id = vendor_id,
        .product_id =product_id,
        .uid = uid,
        .uid2 = {0x00,0x48,0x00,0x3d,0x32,0x35,0x51,0x0f,0x31,0x36,0x33,0x39},
    };

    mavlink_msg_autopilot_version_send_struct(gcsmav->chan, &ver);
}

void gcsmav_send_status_report(gcs_mavlink_t gcsmav)
{
    // prepare flags
    uint16_t flags = 0;
    uitc_estimator_status status;
    union ekf_solution_status ekf_solution_flags;

    if (itc_copy_from_hub(ITC_ID(estimator_status), &status) != 0) {
        return;
    }

    ekf_solution_flags.value = status.solution_status_flags;

    if (ekf_solution_flags.flags.attitude) {
        flags |= EKF_ATTITUDE;
    }
    if (ekf_solution_flags.flags.velocity_horiz) {
        flags |= EKF_VELOCITY_HORIZ;
    }
    if (ekf_solution_flags.flags.velocity_vert) {
        flags |= EKF_VELOCITY_VERT;
    }
    if (ekf_solution_flags.flags.pos_horiz_rel) {
        flags |= EKF_POS_HORIZ_REL;
    }
    if (ekf_solution_flags.flags.pos_horiz_abs) {
        flags |= EKF_POS_HORIZ_ABS;
    }
    if (ekf_solution_flags.flags.pos_vert_abs) {
        flags |= EKF_POS_VERT_ABS;
    }
    if (ekf_solution_flags.flags.pos_vert_agl) {
        flags |= EKF_POS_VERT_AGL;
    }
    if (ekf_solution_flags.flags.const_pos_mode) {
        flags |= EKF_CONST_POS_MODE;
    }
    if (ekf_solution_flags.flags.pred_pos_horiz_rel) {
        flags |= EKF_PRED_POS_HORIZ_REL;
    }
    if (ekf_solution_flags.flags.pred_pos_horiz_abs) {
        flags |= EKF_PRED_POS_HORIZ_ABS;
    }
#if 0
    if (!ekf_solution_status.flags.initalized) {
        flags |= EKF_UNINITIALIZED;
    }
#endif
    if (ekf_solution_flags.flags.gps_glitch) {
        flags |= (1<<15);
    }

    const mavlink_ekf_status_report_t packet = {
        status.vel_test_ratio,
        status.pos_test_ratio,
        status.hgt_test_ratio,
        status.mag_test_ratio,
        status.hagl_test_ratio,
        flags,
        status.tas_test_ratio
    };

    // send message
    mavlink_msg_ekf_status_report_send_struct(gcsmav->chan, &packet);
}

bool gcsmav_try_send_mission_message(gcs_mavlink_t gcsmav, const enum mb_message id)
{
    switch (id) {
    case MAV_MSG_CURRENT_WAYPOINT:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, MISSION_CURRENT);
        gcsmav_send_mission_current(gcsmav, mission_get_current_nav_index());
        break;
    case MAV_MSG_MISSION_ITEM_REACHED:
        MAV_CHECK_PAYLOAD_SIZE(gcsmav, MISSION_ITEM_REACHED);
        mavlink_msg_mission_item_reached_send(gcsmav->chan, gcsmav->mission_item_reached_index);
        break;
#if 0
    case MAV_MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
        gcs_try_send_queued_message_for_type(MAV_MISSION_TYPE_MISSION);
        ret = true;
        break;
#if HAL_RALLY_ENABLED
    case MSG_NEXT_MISSION_REQUEST_RALLY:
        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
        gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_RALLY);
        ret = true;
        break;
#endif
#if AP_FENCE_ENABLED
    case MSG_NEXT_MISSION_REQUEST_FENCE:
        CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
        gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_FENCE);
        ret = true;
        break;
#endif
#endif
    default:
        break;
    }
    return true;
}

void gcsmav_send_accelcal_vehicle_position(gcs_mavlink_t gcsmav, uint32_t position)
{
    if (MAV_HAVE_PAYLOAD_SPACE(gcsmav->chan, COMMAND_LONG)) {
        mavlink_msg_command_long_send(
            gcsmav->chan,
            0,
            0,
            MAV_CMD_ACCELCAL_VEHICLE_POS,
            0,
            (float) position,
            0, 0, 0, 0, 0, 0);
    }
}

// returns true if all battery instances were reported
bool gcsmav_send_battery_status(gcs_mavlink_t gcsmav)
{
    uitc_battery_state battery_state;

    for(uint8_t i = 0; i < MB_BATT_MONITOR_MAX_INSTANCES; i++) {
        const uint8_t battery_id = (gcsmav->last_battery_status_idx + 1) % MB_BATT_MONITOR_MAX_INSTANCES;

        switch (battery_id) {
            case 0:
                itc_copy_from_hub(ITC_ID(battery_state), &battery_state);
                break;

            case 1:
                itc_copy_from_hub(ITC_ID(battery2_state), &battery_state);
                break;

            case 2:
                itc_copy_from_hub(ITC_ID(battery3_state), &battery_state);
                break;

            default:
                break;
        }

        if (battery_state.type != BATT_MONITOR_NONE) {
            MAV_CHECK_PAYLOAD_SIZE(gcsmav, BATTERY_STATUS);
            gcsmav_send_battery_status2(gcsmav, battery_id, &battery_state);
            gcsmav->last_battery_status_idx = battery_id;
            return true;
        } else {
            gcsmav->last_battery_status_idx = battery_id;
        }
    }
    return true;
}

static void gcsmav_send_battery_status2(gcs_mavlink_t gcsmav, const uint8_t instance, uitc_battery_state* battery_state)
{
    // catch the battery backend not supporting the required number of cells
#if 0
    static_assert(sizeof(AP_BattMonitor::cells) >= (sizeof(uint16_t) * MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN),
                  "Not enough battery cells for the MAVLink message");
#endif

    bool got_temperature = (battery_state->temperature_external_use || battery_state->temperature_use);
    float temp = battery_state->temperature_external_use ? battery_state->temperature_external : battery_state->temperature;

    // prepare arrays of individual cell voltages
    uint16_t cell_mvolts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN];
    uint16_t cell_mvolts_ext[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN];
    const uint16_t max_cell_mV = 0xFFFEU;
    const uint16_t invalid_cell_mV = 0xFFFFU;

    if (battery_state->has_cell_voltages) {
        RT_ASSERT(sizeof(cell_mvolts) <= sizeof(battery_state->cell_voltages));

        // copy the first 10 cells
        memcpy(cell_mvolts, battery_state->cell_voltages, sizeof(cell_mvolts));
        // 11 ... 14 use a second cell_volts_ext array
        for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
            if (MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i < (uint8_t)(ARRAY_SIZE(battery_state->cell_voltages))) {
                cell_mvolts_ext[i] = battery_state->cell_voltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i];
            } else {
                cell_mvolts_ext[i] = 0;
            }
        }
        /*
          now adjust voltages to cope with two things:
             1) we may be reporting sag corrected voltage
             2) the battery may have more cells than can be reported by the backend, so the actual voltage may be higher than the sum
        */
        const float voltage_mV = battery_state->gcs_voltage * 1e3f;
        float voltage_mV_sum = 0;
        uint8_t non_zero_cell_count = 0;
        for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
            if (cell_mvolts[i] > 0 && cell_mvolts[i] != invalid_cell_mV) {
                non_zero_cell_count++;
                voltage_mV_sum += cell_mvolts[i];
            }
        }
        for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
            if (cell_mvolts_ext[i] > 0 && cell_mvolts_ext[i] != invalid_cell_mV) {
                non_zero_cell_count++;
                voltage_mV_sum += cell_mvolts_ext[i];
            }
        }
        if (voltage_mV > voltage_mV_sum && non_zero_cell_count > 0) {
            // distribute the extra voltage over the non-zero cells
            uint32_t extra_mV = (voltage_mV - voltage_mV_sum) / non_zero_cell_count;
            for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
                if (cell_mvolts[i] > 0 && cell_mvolts[i] != invalid_cell_mV) {
                    cell_mvolts[i] = MIN(cell_mvolts[i] + extra_mV, max_cell_mV);
                }
            }
            for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
                if (cell_mvolts_ext[i] > 0 && cell_mvolts_ext[i] != invalid_cell_mV) {
                    cell_mvolts_ext[i] = MIN(cell_mvolts_ext[i] + extra_mV, max_cell_mV);
                }
            }
        }
    } else {
        // for battery monitors that cannot provide voltages for individual cells the battery's total voltage is put into the first cell
        // if the total voltage cannot fit into a single field, the remainder into subsequent fields.
        // the GCS can then recover the pack voltage by summing all non ignored cell values an we can report a pack up to 655.34 V
        float voltage_mV = battery_state->gcs_voltage * 1e3f;
        for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
          if (voltage_mV < 0.001f) {
              // too small to send to the GCS, set it to the no cell value
              cell_mvolts[i] = UINT16_MAX;
          } else {
              cell_mvolts[i] = MIN(voltage_mV, max_cell_mV); // Can't send more then UINT16_MAX - 1 in a cell
              voltage_mV -= max_cell_mV;
          }
        }
        for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
            cell_mvolts_ext[i] = 0;
        }
    }

    float current, consumed_mah, consumed_wh;
    const int8_t percentage = battery_state->capacity_remaining_pct;

    if (battery_state->has_current) {
        current = math_constrain_float(battery_state->current_amps * 100,-INT16_MAX,INT16_MAX);
        consumed_mah = battery_state->consumed_mah;
    } else {
        current = -1;
        consumed_mah = -1;
    }

    if (battery_state->has_consumed_energy) {
        consumed_wh = battery_state->consumed_wh * 36;
    } else {
        consumed_wh = -1;
    }

    uint32_t time_remaining;
    if (battery_state->has_time_remaining) {
        time_remaining = battery_state->time_remaining;
    } else {
        time_remaining = 0;
    }

    mavlink_msg_battery_status_send(gcsmav->chan,
                                    instance, // id
                                    MAV_BATTERY_FUNCTION_UNKNOWN, // function
                                    MAV_BATTERY_TYPE_UNKNOWN, // type
                                    got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown
                                    cell_mvolts, // cell voltages
                                    current,      // current in centiampere
                                    consumed_mah, // total consumed current in milliampere.hour
                                    consumed_wh,  // consumed energy in hJ (hecto-Joules)
                                    percentage,
                                    time_remaining, // time remaining, seconds
                                    battery_state->charge_state, // battery charge state
                                    cell_mvolts_ext, // Cell 11..14 voltages
                                    0, // battery mode
                                    battmonitor_get_mavlink_fault_bitmask(instance));   // fault_bitmask
}

static void gcsmav_send_mission_current(gcs_mavlink_t gcsmav, uint16_t seq)
{
    uint16_t num_commands = mission_num_commands();
    if (num_commands > 0) {
        // exclude home location from the count; see message definition.
        num_commands -= 1;
    }

    const uint8_t mission_mode = gcsmav_current_mode_requires_mission(gcsmav) ? 1 : 0;

    mavlink_msg_mission_current_send(
        gcsmav->chan,
        seq,
        num_commands, // total
        gcsmav_mission_state(), // mission_state
        mission_mode);  // mission_mode
}

// returns a MISSION_STATE numeration value best describing out
// current mission state.
static MISSION_STATE gcsmav_mission_state()
{
    if (mission_num_commands() < 2) {  // 1 means just home is present
        return MISSION_STATE_NO_MISSION;
    }
    switch (mission_get_singleton()->_flags.state) {
    case MISSION_STOPPED:
        return MISSION_STATE_NOT_STARTED;
    case MISSION_RUNNING:
        return MISSION_STATE_ACTIVE;
    case MISSION_COMPLETE:
        return MISSION_STATE_COMPLETE;
    }

    // compiler ensures we can't get here as no default case in above enumeration

    return MISSION_STATE_UNKNOWN;
}
/*------------------------------------test------------------------------------*/


